├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── demo_keyframe_bundle_adjustment_meta ├── .gitignore ├── CMakeLists.txt ├── README.md ├── apps │ └── main_program │ │ ├── main_program.cpp │ │ └── utility.h ├── launch │ ├── feature_matching.launch │ ├── keyframe_ba_monolid.launch │ ├── kitti_mono_standalone.launch │ ├── kitti_standalone.launch │ ├── mono_standalone_kitti.launch │ ├── run_deterministic.launch │ ├── semantic_labels.launch │ └── tf2_static_aliases_kitti.launch ├── package.xml ├── res │ ├── config_feature_matching.yaml │ ├── config_tracklets_depth_ros_tool.yaml │ ├── default.rviz │ └── mono_lidar_fusion_parameters.yaml └── src │ └── .gitignore ├── docker ├── docker-compose.yml └── src │ ├── Dockerfile │ └── entry.sh ├── install_repos.sh ├── keyframe_bundle_adjustment ├── CMakeLists.txt ├── README.md ├── include │ └── keyframe_bundle_adjustment │ │ ├── bundle_adjuster_keyframes.hpp │ │ ├── internal │ │ ├── cost_functors_ceres.hpp │ │ ├── definitions.hpp │ │ ├── keyframe_rejection_scheme_flow.hpp │ │ ├── keyframe_schemes_base.hpp │ │ ├── keyframe_selection_scheme_pose.hpp │ │ ├── keyframe_sparsification_scheme_time.hpp │ │ ├── landmark_categorization_interface.hpp │ │ ├── landmark_selection_scheme_add_depth.hpp │ │ ├── landmark_selection_scheme_base.hpp │ │ ├── landmark_selection_scheme_cheirality.hpp │ │ ├── landmark_selection_scheme_dimension_plausibility.hpp │ │ ├── landmark_selection_scheme_helpers.hpp │ │ ├── landmark_selection_scheme_observability.hpp │ │ ├── landmark_selection_scheme_random.hpp │ │ ├── landmark_selection_scheme_voxel.hpp │ │ ├── local_parameterizations.hpp │ │ ├── motion_model_regularization.hpp │ │ └── triangulator.hpp │ │ ├── keyframe.hpp │ │ ├── keyframe_selection_schemes.hpp │ │ ├── keyframe_selector.hpp │ │ ├── landmark_selection_schemes.hpp │ │ └── landmark_selector.hpp ├── package.xml ├── src │ ├── .gitignore │ ├── bundle_adjuster_keyframes.cpp │ ├── definitions.cpp │ ├── keyframe.cpp │ ├── keyframe_rejection_scheme_flow.cpp │ ├── keyframe_selection_scheme_pose.cpp │ ├── keyframe_selector.cpp │ ├── keyframe_sparsification_scheme_time.cpp │ ├── landmark_selection_scheme_add_depth.cpp │ ├── landmark_selection_scheme_cheirality.cpp │ ├── landmark_selection_scheme_helpers.cpp │ ├── landmark_selection_scheme_observability.cpp │ ├── landmark_selection_scheme_random.cpp │ └── landmark_selection_scheme_voxel.cpp └── test │ └── keyframe_bundle_adjustment.cpp ├── keyframe_bundle_adjustment_ros_tool ├── CMakeLists.txt ├── README.md ├── cfg │ ├── MonoLidar.rosif │ └── MonoStandalone.rosif ├── launch │ ├── mono_lidar_node.launch │ ├── mono_lidar_nodelet.launch │ ├── mono_standalone_node.launch │ ├── mono_standalone_nodelet.launch │ └── params │ │ ├── .gitignore │ │ ├── mono_lidar_parameters.yaml │ │ └── mono_standalone_parameters.yaml ├── nodelet_plugins.xml ├── package.xml ├── res │ ├── kitti_eval_script.py │ ├── kitti_eval_script.sh │ ├── outlier_labels.yaml │ └── tune_parameters_kitti.py ├── src │ ├── .gitignore │ ├── commons │ │ ├── color_by_index_hsv.hpp │ │ ├── general_helpers.hpp │ │ └── publish_helpers.hpp │ ├── mono_lidar │ │ ├── mono_lidar.cpp │ │ ├── mono_lidar.hpp │ │ ├── mono_lidar_node.cpp │ │ └── mono_lidar_nodelet.cpp │ └── mono_standalone │ │ ├── mono_standalone.cpp │ │ ├── mono_standalone.hpp │ │ ├── mono_standalone_node.cpp │ │ └── mono_standalone_nodelet.cpp └── test │ ├── mono_lidar_node.cpp │ ├── mono_lidar_node.test │ ├── mono_lidar_nodelet.cpp │ ├── mono_lidar_nodelet.test │ ├── mono_standalone_node.cpp │ ├── mono_standalone_node.test │ ├── mono_standalone_nodelet.cpp │ └── mono_standalone_nodelet.test ├── limo ├── CMakeLists.txt └── package.xml ├── matches_msg_types ├── .gitignore ├── .gitlab-ci.yml ├── CMakeLists.txt ├── README.md ├── include │ └── matches_msg_types │ │ ├── feature_point.hpp │ │ ├── feature_point_depth.hpp │ │ ├── internal │ │ └── .gitignore │ │ ├── tracklet.hpp │ │ └── tracklets.hpp ├── package.xml ├── src │ └── .gitignore └── test │ └── matches_msg_types.cpp ├── pointcloud plotting ├── accumulate_lidar_pcl_from_trajectory_estimate.py ├── vtk_pointcloud.py └── vtk_viewer.py ├── robust_optimization ├── .gitignore ├── CMakeLists.txt ├── README.md ├── docu │ └── structure.svg ├── include │ └── robust_optimization │ │ ├── internal │ │ ├── .gitignore │ │ ├── apply_trimmer.hpp │ │ ├── definitions.hpp │ │ ├── trimmer_fix.hpp │ │ └── trimmer_quantile.hpp │ │ └── robust_solving.hpp ├── package.xml ├── src │ ├── .gitignore │ └── robust_solving.cpp └── test │ └── robust_optimization.cpp ├── src └── .gitkeep └── util_nodes_tf2_ros_tool ├── .gitignore ├── CMakeLists.txt ├── README.md ├── cfg └── StaticTransformAlias.rosif ├── launch ├── params │ ├── .gitignore │ └── static_transform_alias_parameters.yaml ├── static_transform_alias_node.launch └── static_transform_alias_nodelet.launch ├── nodelet_plugins.xml ├── package.xml ├── src ├── .gitignore └── static_transform_alias │ ├── static_transform_alias.cpp │ ├── static_transform_alias.hpp │ ├── static_transform_alias_node.cpp │ └── static_transform_alias_nodelet.cpp └── test ├── test_util_nodes_tf2_ros_tool.cpp └── test_util_nodes_tf2_ros_tool.test /.gitignore: -------------------------------------------------------------------------------- 1 | .catkin_tools 2 | build_limo_release/* 3 | devel_limo_release/* 4 | logs_limo_release/* 5 | 6 | # Prerequisites 7 | *.d 8 | 9 | # Compiled Object files 10 | *.slo 11 | *.lo 12 | *.o 13 | *.obj 14 | 15 | # Precompiled Headers 16 | *.gch 17 | *.pch 18 | 19 | # Compiled Dynamic libraries 20 | *.so 21 | *.dylib 22 | *.dll 23 | 24 | # Fortran module files 25 | *.mod 26 | *.smod 27 | 28 | # Compiled Static libraries 29 | *.lai 30 | *.la 31 | *.a 32 | *.lib 33 | 34 | # Executables 35 | *.exe 36 | *.out 37 | *.app 38 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MRT_PKG_VERSION 4.0.0) 2 | # Modify only if you know what you are doing! 3 | cmake_minimum_required(VERSION 3.5.1) 4 | project(demo_keyframe_bundle_adjustment_meta) 5 | 6 | ################### 7 | ## Find packages ## 8 | ################### 9 | find_package(mrt_cmake_modules REQUIRED) 10 | include(UseMrtStdCompilerFlags) 11 | include(GatherDeps) 12 | 13 | # You can add a custom.cmake in order to add special handling for this package. E.g. you can do: 14 | # list(REMOVE_ITEM DEPENDEND_PACKAGES ...) 15 | # To remove libs which cannot be found automatically. You can also "find_package" other, custom dependencies there. 16 | # You can also set PROJECT_INSTALL_FILES to install files that are not installed by default. 17 | if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake") 18 | include("${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake") 19 | endif() 20 | 21 | find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES}) 22 | 23 | mrt_parse_package_xml() 24 | 25 | ######################## 26 | ## Add python modules ## 27 | ######################## 28 | # This adds a python module if located under src/{PROJECT_NAME) 29 | mrt_python_module_setup() 30 | 31 | 32 | ########### 33 | ## Build ## 34 | ########### 35 | mrt_glob_folders(SRC_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/src") 36 | if (SRC_DIRECTORIES) 37 | # Found subfolders, add executable for each subfolder 38 | foreach(SRC_DIR ${SRC_DIRECTORIES}) 39 | mrt_add_executable(${SRC_DIR} FOLDER "src/${SRC_DIR}") 40 | endforeach() 41 | else() 42 | # No subfolder found, add executable and python modules for src folder 43 | mrt_add_executable(${PROJECT_NAME} FOLDER "src") 44 | endif() 45 | 46 | ############# 47 | ## Install ## 48 | ############# 49 | # Install all targets, headers by default and scripts and other files if specified (folders or files). 50 | # This command also exports libraries and config files for dependent packages and this supersedes catkin_package. 51 | mrt_install(PROGRAMS scripts FILES res data ${PROJECT_INSTALL_FILES}) 52 | 53 | ############# 54 | ## Testing ## 55 | ############# 56 | # Add test targets for cpp and python tests 57 | if (CATKIN_ENABLE_TESTING) 58 | mrt_add_tests(test) 59 | mrt_add_nosetests(test) 60 | endif() 61 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | setup.py 3 | *.pyc 4 | CMakeLists.txt.user 5 | *.orig 6 | 7 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MRT_PKG_VERSION 2.2) 2 | # Modify only if you know what you are doing! 3 | cmake_minimum_required(VERSION 2.8.12) 4 | project(demo_keyframe_bundle_adjustment_meta) 5 | 6 | ################### 7 | ## find packages ## 8 | ################### 9 | find_package(mrt_cmake_modules REQUIRED) 10 | include(UseMrtStdCompilerFlags) 11 | include(UseMrtAutoTarget) 12 | 13 | include(GatherDeps) 14 | # Remove libs which cannot be found automatically 15 | #list(REMOVE_ITEM DEPENDEND_PACKAGES ...) 16 | find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES}) 17 | 18 | # Manually resolve removed dependend packages 19 | #find_package(...) 20 | 21 | # Mark files or folders for display in IDEs 22 | mrt_add_to_ide(README.md .gitlab-ci.yml) 23 | 24 | ######################## 25 | ## add python modules ## 26 | ######################## 27 | # This adds a python module if located under src/{PROJECT_NAME) 28 | mrt_python_module_setup() 29 | 30 | ################################################ 31 | ## Declare ROS messages, services and actions ## 32 | ################################################ 33 | 34 | # Add message, service and action files 35 | glob_ros_files(add_message_files msg) 36 | glob_ros_files(add_service_files srv) 37 | glob_ros_files(add_action_files action) 38 | 39 | # Generate added messages and services with any dependencies listed here 40 | if (ROS_GENERATE_MESSAGES) 41 | generate_messages( 42 | DEPENDENCIES 43 | #add dependencies here 44 | #std_msgs 45 | ) 46 | endif() 47 | 48 | # Generate dynamic reconfigure options 49 | file(GLOB PARAMS_FILES RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "cfg/*.params" "cfg/*.cfg" "cfg/*.mrtcfg" "cfg/*.rosif") 50 | if (PARAMS_FILES) 51 | generate_ros_parameter_files(${PARAMS_FILES}) 52 | generate_ros_interface_files(${PARAMS_FILES}) 53 | endif () 54 | 55 | ################################### 56 | ## catkin specific configuration ## 57 | ################################### 58 | catkin_package( 59 | ) 60 | 61 | ########### 62 | ## Build ## 63 | ########### 64 | # Add include and library directories 65 | include_directories( 66 | ${mrt_INCLUDE_DIRS} 67 | ${catkin_INCLUDE_DIRS} 68 | ) 69 | 70 | link_directories( 71 | ${mrt_LIBRARY_DIRS} 72 | ) 73 | 74 | glob_folders(SRC_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/src") 75 | if (SRC_DIRECTORIES) 76 | # Found subfolders, add executable for each subfolder 77 | foreach(SRC_DIR ${SRC_DIRECTORIES}) 78 | mrt_add_node_and_nodelet(${SRC_DIR} FOLDER "src/${SRC_DIR}") 79 | endforeach() 80 | else() 81 | # No subfolder found, add executable and python modules for src folder 82 | mrt_add_node_and_nodelet(${PROJECT_NAME} FOLDER "src") 83 | endif() 84 | 85 | 86 | mrt_glob_folders(APP_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/apps") 87 | if (APP_DIRECTORIES) 88 | # Found subfolders, add executable for each subfolder 89 | foreach(APP_DIR ${APP_DIRECTORIES}) 90 | mrt_add_executable(${APP_DIR} FOLDER "apps/${APP_DIR}") 91 | endforeach() 92 | else() 93 | # No subfolder found, add executable and python modules for src folder 94 | mrt_add_executable(${PROJECT_NAME} FOLDER "apps") 95 | endif() 96 | ############# 97 | ## Install ## 98 | ############# 99 | # Install all targets, headers by default and scripts and other files if specified (folders or files) 100 | mrt_install(PROGRAMS scripts FILES launch rviz maps res data nodelet_plugins.xml plugin_description.xml) 101 | 102 | ############# 103 | ## Testing ## 104 | ############# 105 | # Add test targets for cpp and python tests 106 | if (CATKIN_ENABLE_TESTING) 107 | # Include header in src folder via src//.h 108 | include_directories("src") 109 | mrt_add_ros_tests(test) 110 | mrt_add_nosetests(test) 111 | endif() 112 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/README.md: -------------------------------------------------------------------------------- 1 | # demo keyframe bundle adjustment meta 2 | 3 | This is a demo package with launch files for keyframe bundle adjustment with kitti 4 | 5 | * mrtmount 6 | * launch kitti_standalone 7 | 8 | ## Credits 9 | 10 | Johannes Gräter -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/apps/main_program/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | bool read_lidar_data(const std::string lidar_data_path, 12 | std::vector &lidar_points, 13 | std::vector &lidar_intensities, 14 | pcl::PointCloud &laser_cloud) { 15 | std::ifstream lidar_data_file(lidar_data_path, 16 | std::ifstream::in | std::ifstream::binary); 17 | if (!lidar_data_file.is_open()) { 18 | return false; 19 | } 20 | lidar_data_file.seekg(0, std::ios::end); 21 | const size_t num_elements = lidar_data_file.tellg() / sizeof(float); 22 | lidar_data_file.seekg(0, std::ios::beg); 23 | 24 | std::vector lidar_data_buffer(num_elements); 25 | lidar_data_file.read(reinterpret_cast(&lidar_data_buffer[0]), 26 | num_elements * sizeof(float)); 27 | 28 | for (std::size_t i = 0; i < lidar_data_buffer.size(); i += 4) { 29 | pcl::PointXYZI point; 30 | point.x = lidar_data_buffer[i]; 31 | point.y = lidar_data_buffer[i + 1]; 32 | point.z = lidar_data_buffer[i + 2]; 33 | point.intensity = lidar_data_buffer[i + 3]; 34 | laser_cloud.push_back(point); 35 | 36 | lidar_points.emplace_back(point.x, point.y, point.z); 37 | lidar_intensities.push_back(point.intensity); 38 | } 39 | return true; 40 | } 41 | 42 | bool read_gt_pose(const std::string &value_string, 43 | const Eigen::Quaterniond &q_transform, float timestamp, 44 | nav_msgs::Odometry &odomGT, nav_msgs::Path &pathGT) { 45 | std::stringstream pose_stream(value_string); 46 | std::string s; 47 | // Eigen::Matrix gt_pose; 48 | Eigen::Affine3d gt_pose; 49 | geometry_msgs::PoseStamped poseGT; 50 | 51 | for (std::size_t i = 0; i < 3; ++i) { 52 | for (std::size_t j = 0; j < 4; ++j) { 53 | std::getline(pose_stream, s, ' '); 54 | if (s.empty()) return false; 55 | gt_pose.matrix()(i, j) = stof(s); 56 | } 57 | } 58 | 59 | Eigen::Quaterniond q_w_i(gt_pose.matrix().topLeftCorner<3, 3>()); 60 | Eigen::Quaterniond q = q_transform * q_w_i; 61 | q.normalize(); 62 | Eigen::Vector3d t = q_transform * gt_pose.matrix().topRightCorner<3, 1>(); 63 | 64 | odomGT.header.stamp = ros::Time().fromSec(timestamp); 65 | odomGT.pose.pose.orientation.x = q.x(); 66 | odomGT.pose.pose.orientation.y = q.y(); 67 | odomGT.pose.pose.orientation.z = q.z(); 68 | odomGT.pose.pose.orientation.w = q.w(); 69 | odomGT.pose.pose.position.x = t(0); 70 | odomGT.pose.pose.position.y = t(1); 71 | odomGT.pose.pose.position.z = t(2); 72 | 73 | poseGT.header = odomGT.header; 74 | poseGT.pose = odomGT.pose.pose; 75 | pathGT.header.stamp = odomGT.header.stamp; 76 | pathGT.poses.push_back(poseGT); 77 | return true; 78 | } -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/launch/feature_matching.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 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/launch/keyframe_ba_monolid.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/launch/kitti_mono_standalone.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/launch/kitti_standalone.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/launch/mono_standalone_kitti.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 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/launch/run_deterministic.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 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/launch/semantic_labels.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/launch/tf2_static_aliases_kitti.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 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | demo_keyframe_bundle_adjustment_meta 4 | 0.0.0 5 | ToDo: Edit package description 6 | 7 | contact-the-maintainer 8 | Johannes Graeter 9 | Johannes Graeter 10 | 11 | catkin 12 | mrt_cmake_modules 13 | rosinterface_handler 14 | message_generation 15 | message_runtime 16 | gtest 17 | rostest 18 | 19 | roscpp 20 | 21 | roslib 22 | nodelet 23 | mrt_pcl 24 | image_transport 25 | rosbag 26 | mrt_opencv 27 | cv_bridge 28 | 29 | util_nodes_tf2_ros_tool 30 | image_preproc_ros_tool 31 | image_preproc 32 | keyframe_bundle_adjustment_ros_tool 33 | tracklets_depth_ros_tool 34 | matches_conversion_ros_tool 35 | viso_feature_tracking_ros_tool 36 | 37 | viso_feature_tracking 38 | tracklets_depth 39 | monolidar_fusion 40 | matches_msg_ros 41 | yaml-cpp 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/res/config_feature_matching.yaml: -------------------------------------------------------------------------------- 1 | # ROS interface settings 2 | general: 3 | matches_msg_queue_size: 10 4 | matches_msg_name: tgt/matches 5 | image_msg_queue_size: 10 6 | image_msg_name: src/image 7 | scale_factor: 1.0 # Factor used to scale down image. 8 | mask_path: "" # Path to mask (png with black and white) 9 | 10 | # viso2::Matcher params and tracker config 11 | matcher: 12 | nms_n: 9 13 | nms_tau: 20 14 | match_binsize: 200 15 | match_radius: 400 16 | outlier_flow_tolerance: 4 17 | multi_stage: 1 18 | half_resolution: 0 19 | max_track_length: 100 20 | method: 0 21 | refinement: 1 #0: none, 1: pixel, 2: subpixel 22 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/res/config_tracklets_depth_ros_tool.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # _____ Subscriber _____ 4 | 5 | subscriber_msg_name_cloud: /sensor/velodyne/cloud_euclidean 6 | subscriber_msg_name_tracklets: /matches/grayscale/left 7 | subscriber_msg_name_camera_info: /sensor/camera/grayscale/left/camera_info 8 | # subscriber_msg_name_semantics: /sensor/camera/color_labels/left/image_rect 9 | 10 | # _____ Publisher _____ 11 | 12 | publisher_msg_name_cloud_interpolated: /monolidar_debug/cloud_interpolated 13 | publisher_msg_name_cloud_camera_cs: /monolidar_debug/cloud_camera_cs 14 | publisher_msg_name_image_depth: /monolidar_debug/image_depth 15 | publisher_msg_name_image_projection_cloud: /monolidar_debug/image_cloud_projection 16 | publisher_msg_name_depthcalc_stats: /monolidar_debug/depthcalc_stats 17 | publisher_msg_name_tracklets_depth: /tracklets_depth/tracklets/left 18 | 19 | image_depth_cam_point_distance: 30 20 | image_depth_cam_fontsize: 0.35 21 | 22 | # _____ Frame names _____ 23 | 24 | tf_frame_name_cameraLeft: sensor/camera 25 | tf_frame_name_velodyne: sensor/velodyne 26 | 27 | # _____ Program flow _____ 28 | 29 | # 1: publish pointcloud in camera cs; 0: do not publish (saves processing ressources) 30 | do_publish_cloud_camera_cs: 0 31 | 32 | # 1: publish projection of the pointcloud into the image frame of the camera; 0: do not publish (saves processing ressources) 33 | do_publish_image_projection_cloud: 0 34 | 35 | # 1: publish interpolated points (which are the estimated 3d points for selected points (mostly feature points) from the image cs) 36 | do_publish_cloud_interpolated: 0 37 | 38 | # 1: publishes statistics how many depth calculations of the feature points succeeded and by which pipeline step they were rejected if not 39 | do_publish_stats: 0 40 | 41 | # 1: publishes a depth image of the calculated depths 42 | do_publish_image_depth: 0 43 | 44 | 45 | # _____ Misc _____ 46 | 47 | msg_queue_size: 10 48 | -------------------------------------------------------------------------------- /demo_keyframe_bundle_adjustment_meta/src/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/demo_keyframe_bundle_adjustment_meta/src/.gitignore -------------------------------------------------------------------------------- /docker/docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: '3.4' 2 | 3 | services: 4 | limo: 5 | container_name: limo 6 | build: 7 | context: ./src 8 | image: ros:melodic-perception 9 | ports: 10 | - 8888:8888 11 | ipc: host 12 | privileged: true 13 | runtime: nvidia 14 | environment: 15 | - DISPLAY=unix${DISPLAY} 16 | - NVIDIA_VISIBLE_DEVICES=all 17 | - NVIDIA_DRIVER_CAPABILITIES=all 18 | volumes: 19 | - $HOME/limo_data:/limo_data 20 | - /tmp/.X11-unix:/tmp/.X11-unix 21 | # network_mode: host # error: conflicting options: host type networking can't be used with links comment to avoid it 22 | stdin_open: true 23 | tty: true 24 | -------------------------------------------------------------------------------- /docker/src/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic-perception 2 | 3 | # System settings 4 | # max watches on files (commented out because it's not necessary right now) 5 | # RUN echo "fs.inotify.max_user_watches = 524288" >> /etc/sysctl.conf && sysctl -p --system 6 | 7 | # Install some basic utilities 8 | RUN apt update && apt install --no-install-recommends -y \ 9 | build-essential \ 10 | cmake \ 11 | libopencv-dev \ 12 | libgoogle-glog-dev \ 13 | libatlas-base-dev \ 14 | libeigen3-dev \ 15 | libsuitesparse-dev \ 16 | libpng++-dev \ 17 | git \ 18 | libx11-6 \ 19 | libgtk2.0-0 \ 20 | vim \ 21 | nano \ 22 | wget \ 23 | python-catkin-tools \ 24 | ros-melodic-opencv-apps \ 25 | ros-melodic-diagnostic-updater \ 26 | ros-melodic-tf-conversions \ 27 | ros-melodic-rviz \ 28 | lcov \ 29 | tmux \ 30 | python-pip \ 31 | && apt clean \ 32 | && rm -rf /var/lib/apt/lists/* \ 33 | && rm -rf /tmp/* 34 | 35 | RUN pip install --upgrade pip 36 | 37 | RUN pip install pykitti \ 38 | jupyter \ 39 | jupytext --upgrade 40 | 41 | # Install ceres solver 42 | # Why is rm necessary? remove it when solved. 43 | # WORKDIR /opt 44 | # RUN rm -rf /opt/ceres-solver \ 45 | # && git clone https://ceres-solver.googlesource.com/ceres-solver \ 46 | # && mkdir -p /opt/ceres-solver/build 47 | RUN apt update && apt install --no-install-recommends -y libceres-dev 48 | 49 | # WORKDIR /opt/ceres-solver/build 50 | # RUN cmake .. -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF \ 51 | # && make \ 52 | # && make install 53 | 54 | # Make catkin workspace 55 | RUN rm -rf /workspace \ 56 | && mkdir -p /workspace/limo_ws/src \ 57 | && /bin/bash -c 'source /opt/ros/melodic/setup.bash && catkin_init_workspace /workspace/limo_ws/src' 58 | 59 | # WORKDIR /workspace/limo_ws 60 | # RUN catkin config --profile limo_release -x _limo_release --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_FLAGS=-Wall -Wextra -Wno-unused-parameter -Werror=address -Werror=array-bounds=1 -Werror=bool-compare -Werror=comment -Werror=enum-compare -Werror=format -Werror=init-self -Werror=logical-not-parentheses -Werror=maybe-uninitialized -Werror=memset-transposed-args -Werror=nonnull -Werror=nonnull-compare -Werror=openmp-simd -Werror=parentheses -Werror=return-type -Werror=sequence-point -Werror=sizeof-pointer-memaccess -Werror=switch -Werror=tautological-compare -Werror=trigraphs -Werror=uninitialized -Werror=volatile-register-var 61 | 62 | # Install limo 63 | WORKDIR /workspace/limo_ws/src 64 | RUN rm -rf /workspace/limo_ws/src/limo \ 65 | && git clone https://github.com/johannes-graeter/limo.git \ 66 | && rm -rf /workspace/limo_ws/src/feature_tracking \ 67 | && git clone https://github.com/johannes-graeter/feature_tracking.git \ 68 | && cd feature_tracking \ 69 | && git checkout python_binding \ 70 | && cd .. \ 71 | && rm -rf /workspace/limo_ws/src/mono_lidar_depth \ 72 | && git clone https://github.com/johannes-graeter/mono_lidar_depth.git \ 73 | && rm -rf /workspace/limo_ws/src/viso2.git \ 74 | && git clone https://github.com/johannes-graeter/viso2.git \ 75 | && rm -rf /workspace/limo_ws/src/mrt_cmake_modules \ 76 | && git clone https://github.com/johannes-graeter/mrt_cmake_modules.git \ 77 | && rm -rf /workspace/limo_ws/src/rosinterface_handler \ 78 | && git clone https://github.com/johannes-graeter/rosinterface_handler.git 79 | 80 | # solve issue https://github.com/johannes-graeter/limo/issues/50 81 | RUN cp -rf /usr/include/eigen3/Eigen /usr/include/Eigen -R 82 | RUN ln -sf /usr/include/eigen3/Eigen /usr/include/Eigen 83 | 84 | WORKDIR /workspace/limo_ws/ 85 | RUN /bin/bash -c 'source /opt/ros/melodic/setup.bash && catkin build' 86 | 87 | WORKDIR /workspace/limo_ws/src/feature_tracking/feature_tracking_core/notebook 88 | RUN jupytext feature_tracking_notebook.py --to notebook 89 | 90 | COPY entry.sh /entry.sh 91 | RUN chmod +x /entry.sh 92 | 93 | ENTRYPOINT ["/entry.sh"] 94 | # CMD ["bash"] 95 | CMD /bin/bash -c 'source /opt/ros/melodic/setup.bash && source /workspace/limo_ws/devel/setup.bash && jupyter notebook --port=8888 --no-browser --ip=0.0.0.0 --allow-root /workspace/limo_ws/src/feature_tracking/feature_tracking_core/notebook/feature_tracking_notebook.ipynb' 96 | -------------------------------------------------------------------------------- /docker/src/entry.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # setup ros environment 5 | source "/opt/ros/melodic/setup.bash" 6 | exec "$@" 7 | -------------------------------------------------------------------------------- /install_repos.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # make catkin profile 3 | catkin config --profile limo_release -x _limo_release --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_FLAGS=-Wall -Wextra -Wno-unused-parameter -Werror=address -Werror=array-bounds=1 -Werror=bool-compare -Werror=comment -Werror=enum-compare -Werror=format -Werror=init-self -Werror=logical-not-parentheses -Werror=maybe-uninitialized -Werror=memset-transposed-args -Werror=nonnull -Werror=nonnull-compare -Werror=openmp-simd -Werror=parentheses -Werror=return-type -Werror=sequence-point -Werror=sizeof-pointer-memaccess -Werror=switch -Werror=tautological-compare -Werror=trigraphs -Werror=uninitialized -Werror=volatile-register-var 4 | 5 | # clone stuff 6 | cd .. 7 | mkdir src 8 | git clone https://github.com/johannes-graeter/feature_tracking.git 9 | git clone https://github.com/johannes-graeter/mono_lidar_depth.git 10 | git clone https://github.com/johannes-graeter/viso2.git 11 | git clone https://github.com/KIT-MRT/mrt_cmake_modules.git 12 | git clone https://github.com/KIT-MRT/rosinterface_handler.git 13 | 14 | # build it with profile 15 | catkin build --profile limo_release 16 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MRT_PKG_VERSION 2.2) 2 | # Modify only if you know what you are doing! 3 | cmake_minimum_required(VERSION 2.8.12) 4 | project(keyframe_bundle_adjustment) 5 | 6 | ################### 7 | ## find packages ## 8 | ################### 9 | find_package(mrt_cmake_modules REQUIRED) 10 | set(MRT_ARCH "None") 11 | include(UseMrtStdCompilerFlags) 12 | include(UseMrtAutoTarget) 13 | 14 | include(GatherDeps) 15 | # Remove libs which cannot be found automatically 16 | #list(REMOVE_ITEM DEPENDEND_PACKAGES ...) 17 | find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES}) 18 | 19 | # Manually resolve removed dependend packages 20 | #find_package(...) 21 | 22 | # Mark files or folders for display in IDEs 23 | mrt_add_to_ide(README.md .gitlab-ci.yml) 24 | 25 | ######################## 26 | ## add python modules ## 27 | ######################## 28 | # This adds a python module if located under src/{PROJECT_NAME) 29 | mrt_python_module_setup() 30 | 31 | file(GLOB PROJECT_PYTHON_SOURCE_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "python_api/*.cpp") 32 | if (PROJECT_PYTHON_SOURCE_FILES_SRC) 33 | # Add a cpp-python api library. Make sure there are no name collisions with python modules in this project 34 | mrt_add_python_api( ${PROJECT_NAME} 35 | FILES ${PROJECT_PYTHON_SOURCE_FILES_SRC} 36 | ) 37 | endif() 38 | 39 | ############################ 40 | ## read source code files ## 41 | ############################ 42 | file(GLOB_RECURSE PROJECT_HEADER_FILES_INC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "include/*.h" "include/*.hpp") 43 | file(GLOB PROJECT_SOURCE_FILES_INC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "src/*.h" "src/*.hpp") 44 | file(GLOB PROJECT_SOURCE_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "src/*.cpp") 45 | 46 | if (PROJECT_SOURCE_FILES_SRC) 47 | set(LIBRARY_NAME ${PROJECT_NAME}) 48 | endif() 49 | 50 | ################################### 51 | ## Eigen 52 | #add_definitions(-DEIGEN_DONT_VECTORIZE -DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT) 53 | 54 | ################################### 55 | ## catkin specific configuration ## 56 | ################################### 57 | # The catkin_package macro generates cmake config files for your package 58 | # Declare things to be passed to dependent projects 59 | # INCLUDE_DIRS: uncomment this if you package contains header files 60 | # LIBRARIES: libraries you create in this project that dependent projects also need 61 | # CATKIN_DEPENDS: catkin_packages dependent projects also need 62 | # DEPENDS: system dependencies of this project that dependent projects also need 63 | catkin_package( 64 | INCLUDE_DIRS include ${mrt_EXPORT_INCLUDE_DIRS} 65 | LIBRARIES ${LIBRARY_NAME} ${mrt_EXPORT_LIBRARIES} 66 | CATKIN_DEPENDS ${catkin_EXPORT_DEPENDS} 67 | ) 68 | 69 | ########### 70 | ## Build ## 71 | ########### 72 | # Add include and library directories 73 | include_directories( 74 | include/${PROJECT_NAME} 75 | ${mrt_INCLUDE_DIRS} 76 | ${catkin_INCLUDE_DIRS} 77 | ) 78 | 79 | link_directories( 80 | ${mrt_LIBRARY_DIRS} 81 | ) 82 | 83 | # Declare a cpp library 84 | mrt_add_library(${PROJECT_NAME} 85 | INCLUDES ${PROJECT_HEADER_FILES_INC} ${PROJECT_SOURCE_FILES_INC} 86 | SOURCES ${PROJECT_SOURCE_FILES_SRC} 87 | ) 88 | 89 | ############# 90 | ## Install ## 91 | ############# 92 | # Install all targets, headers by default and scripts and other files if specified (folders or files) 93 | mrt_install(PROGRAMS scripts FILES res data) 94 | 95 | ############# 96 | ## Testing ## 97 | ############# 98 | # Add test targets for cpp and python tests 99 | if (CATKIN_ENABLE_TESTING) 100 | mrt_add_tests(test) 101 | mrt_add_nosetests(test) 102 | endif() 103 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/keyframe_rejection_scheme_flow.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "keyframe_schemes_base.hpp" 12 | 13 | namespace keyframe_bundle_adjustment { 14 | 15 | /** 16 | * @class KeyframeSelectionSchemeFlow 17 | * @par 18 | * 19 | * select keyframes from flow, this is particularly important for stand still detection to not add 20 | * frames between which flow is too small 21 | */ 22 | class KeyframeRejectionSchemeFlow : public KeyframeRejectionSchemeBase { 23 | public: // public classes/enums/types etc... 24 | public: // attributes 25 | public: // public methods 26 | // default constructor 27 | KeyframeRejectionSchemeFlow(double min_median_flow); 28 | 29 | // default destructor 30 | virtual ~KeyframeRejectionSchemeFlow() = default; 31 | 32 | // default move 33 | KeyframeRejectionSchemeFlow(KeyframeRejectionSchemeFlow&& other) = default; 34 | KeyframeRejectionSchemeFlow& operator=(KeyframeRejectionSchemeFlow&& other) = default; 35 | 36 | // default copy 37 | KeyframeRejectionSchemeFlow(const KeyframeRejectionSchemeFlow& other) = default; 38 | KeyframeRejectionSchemeFlow& operator=(const KeyframeRejectionSchemeFlow& other) = default; 39 | 40 | ////////////////////////////////////////////////// 41 | /// \brief isSelected, concrete selection scheme, according to interface in base 42 | /// calculates medioan of flow to most recent keyframe from last_frames and from all cameras 43 | /// \param new_frame, frame that should be tested for selection 44 | /// \param last_frames, frames, against which new_frame is tested 45 | /// \return true if median flow is bigger than min_median_flow_ 46 | /// 47 | bool isUsable(const Keyframe::Ptr& new_frame, 48 | const std::map& last_frames) const override; 49 | 50 | ////////////////////////////////////////////////// 51 | /// \brief create_const, factory function for const pointer 52 | /// \param time_difference_nano_sec_ 53 | /// \return 54 | /// 55 | static ConstPtr createConst(double min_median_flow); 56 | 57 | ////////////////////////////////////////////////// 58 | /// \brief create, factory function 59 | /// \param time_difference_nano_sec_ 60 | /// \return 61 | /// 62 | static Ptr create(double min_median_flow); 63 | 64 | public: 65 | double min_median_flow_squared_; ///< when median of flow most recent keyframe is smaller than 66 | /// this threshold, reject frame 67 | }; 68 | } 69 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/keyframe_selection_scheme_pose.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "keyframe_schemes_base.hpp" 12 | 13 | namespace keyframe_bundle_adjustment { 14 | 15 | /** 16 | * @class KeyframeSelectionSchemePose 17 | * @par 18 | * 19 | * This scheme selects keyframes that have a pose difference over a certain threshold. This is 20 | * important in curves where many keyframes are needed to not loose track. 21 | */ 22 | class KeyframeSelectionSchemePose : public KeyframeSelectionSchemeBase { 23 | public: // public classes/enums/types etc... 24 | public: // attributes 25 | public: // public methods 26 | // default constructor 27 | KeyframeSelectionSchemePose(double critical_quaternion_difference); 28 | 29 | // default destructor 30 | virtual ~KeyframeSelectionSchemePose() = default; 31 | 32 | // default move 33 | KeyframeSelectionSchemePose(KeyframeSelectionSchemePose&& other) = default; 34 | KeyframeSelectionSchemePose& operator=(KeyframeSelectionSchemePose&& other) = default; 35 | 36 | // default copy 37 | KeyframeSelectionSchemePose(const KeyframeSelectionSchemePose& other) = default; 38 | KeyframeSelectionSchemePose& operator=(const KeyframeSelectionSchemePose& other) = default; 39 | 40 | ////////////////////////////////////////////////// 41 | /// \brief isSelected, concrete selection scheme, according to interface in base 42 | /// \param 43 | /// \param 44 | /// 45 | bool isUsable(const Keyframe::Ptr& new_frame, 46 | const std::map& last_frames) const override; 47 | 48 | ////////////////////////////////////////////////// 49 | /// \brief create_const, factory function for const pointer 50 | /// \param time_difference_nano_sec_ 51 | /// \return 52 | /// 53 | static ConstPtr createConst(double critical_quaternion_difference); 54 | 55 | ////////////////////////////////////////////////// 56 | /// \brief create, factory function 57 | /// \param time_difference_nano_sec_ 58 | /// \return 59 | /// 60 | static Ptr create(double critical_quaternion_difference); 61 | 62 | private: 63 | double critical_quaternion_diff_; ///< if difference angle between quaternions is 64 | }; 65 | } 66 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/keyframe_sparsification_scheme_time.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "keyframe_schemes_base.hpp" 12 | 13 | namespace keyframe_bundle_adjustment { 14 | 15 | /** 16 | * @class KeyframeSelectionSchemeTime 17 | * @par 18 | * 19 | * Concrete KeyframeSelectionScheme; select keyframes on the time between them 20 | */ 21 | class KeyframeSparsificationSchemeTime : public KeyframeSparsificationSchemeBase { 22 | public: // public classes/enums/types etc... 23 | using DurationNSec = unsigned long; 24 | 25 | public: // public methods 26 | // default constructor 27 | KeyframeSparsificationSchemeTime(double time_difference_sec) 28 | : time_difference_nano_sec_(convert(time_difference_sec)) { 29 | ; 30 | } 31 | 32 | // default destructor 33 | virtual ~KeyframeSparsificationSchemeTime() = default; 34 | 35 | // default move 36 | KeyframeSparsificationSchemeTime(KeyframeSparsificationSchemeTime&& other) = default; 37 | KeyframeSparsificationSchemeTime& operator=(KeyframeSparsificationSchemeTime&& other) = default; 38 | 39 | // default copy 40 | KeyframeSparsificationSchemeTime(const KeyframeSparsificationSchemeTime& other) = default; 41 | KeyframeSparsificationSchemeTime& operator=(const KeyframeSparsificationSchemeTime& other) = default; 42 | 43 | ////////////////////////////////////////////////// 44 | /// \brief isSelected, concrete selection scheme, according to interface in base 45 | /// \param 46 | /// \param 47 | /// 48 | bool isUsable(const Keyframe::Ptr& new_frame, 49 | const std::map& last_frames) const override; 50 | 51 | ////////////////////////////////////////////////// 52 | /// \brief create_const, factory function for const pointer 53 | /// \param time_difference_nano_sec_ 54 | /// \return 55 | /// 56 | static ConstPtr createConst(double time_difference_nano_sec_); 57 | 58 | ////////////////////////////////////////////////// 59 | /// \brief create, factory function 60 | /// \param time_difference_nano_sec_ 61 | /// \return 62 | /// 63 | static Ptr create(double time_difference_nano_sec_); 64 | 65 | 66 | public: // attributes 67 | DurationNSec time_difference_nano_sec_; 68 | }; 69 | } 70 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/landmark_categorization_interface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "definitions.hpp" 12 | #include "../keyframe.hpp" 13 | 14 | namespace keyframe_bundle_adjustment { 15 | 16 | /** 17 | * @class LandmarkCategorizatonInterface 18 | * @par 19 | * 20 | * A class inherting from this will be forced to have an interface that supplies categories for 21 | * landmarks 22 | */ 23 | struct LandmarkCategorizatonInterface { 24 | enum class Category { NearField, MiddleField, FarField }; 25 | 26 | virtual std::map getCategorizedSelection( 27 | const std::map& landmarks, 28 | const std::map& keyframes) const = 0; 29 | }; 30 | } 31 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/landmark_selection_scheme_add_depth.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include 12 | #include "landmark_selection_scheme_base.hpp" 13 | 14 | namespace keyframe_bundle_adjustment { 15 | 16 | /** 17 | * @class LandmarkSelectionSchemeAddWDepth 18 | * @par 19 | * 20 | * Through all the seelction algorithms it can happen, that we don not have enough depth measurements. 21 | * This class assures that we have enough depth measruements in the selection and adds ids of landmarks 22 | * even though they have been removed before. 23 | */ 24 | class LandmarkSelectionSchemeAddDepth : public LandmarkSelectionSchemeBase { 25 | public: // public classes/enums/types etc... 26 | using LmMap = Eigen::Map; 27 | using FrameIndex = int; ///< Index of frame, 0 means first frame. 28 | using NumberLandmarks = int; ///< Number of landmarks to take 29 | using Comparator = 30 | std::function; ///< Function which determines if shall be adde dor not. 31 | using Sorter = std::function; ///< Function that 32 | /// gives a double for 33 | /// which the elements 34 | /// are sorted. 35 | struct Parameters { 36 | std::vector> 37 | params_per_keyframe; ///< Determines how many depth points are assured on what keyframes. 38 | // { 39 | // std::make_tuple(0, 40 | // 20, 41 | // [](const Landmark::ConstPtr& lm) { return lm->has_measured_depth; }, 42 | // [](const Measurement& m, const Eigen::Vector3d& lm) { return m.d; }), 43 | // std::make_tuple(0, 44 | // 20, 45 | // [](const Landmark::ConstPtr& lm) { return lm->is_ground_plane; }, 46 | // [](const Measurement& m, const Eigen::Vector3d& local_lm) { 47 | // return local_lm.norm(); 48 | // })}; 49 | }; 50 | 51 | public: // public methods 52 | // default constructor 53 | LandmarkSelectionSchemeAddDepth(Parameters p) : params_(p) { 54 | identifier = "add depth"; 55 | } 56 | 57 | // default destructor 58 | ~LandmarkSelectionSchemeAddDepth() = default; 59 | 60 | // default move 61 | LandmarkSelectionSchemeAddDepth(LandmarkSelectionSchemeAddDepth&& other) = default; 62 | LandmarkSelectionSchemeAddDepth& operator=(LandmarkSelectionSchemeAddDepth&& other) = default; 63 | 64 | // default copy 65 | LandmarkSelectionSchemeAddDepth(const LandmarkSelectionSchemeAddDepth& other) = default; 66 | LandmarkSelectionSchemeAddDepth& operator=(const LandmarkSelectionSchemeAddDepth& other) = default; 67 | 68 | std::set getSelection(const LandmarkMap& landmarks, const KeyframeMap& keyframes) const override; 69 | 70 | ////////////////////////////////////////////////// 71 | /// \brief createConst, factory function for const pointer 72 | /// \return 73 | /// 74 | static ConstPtr createConst(Parameters p); 75 | 76 | ////////////////////////////////////////////////// 77 | /// \brief create, factory function 78 | /// \return 79 | /// 80 | static Ptr create(Parameters p); 81 | 82 | 83 | public: // attributes 84 | Parameters params_; ///< paramters 85 | }; 86 | } 87 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/landmark_selection_scheme_cheirality.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "landmark_selection_scheme_base.hpp" 12 | 13 | namespace keyframe_bundle_adjustment { 14 | 15 | /** 16 | * @class LandmarkSelectionSchemeCheirality 17 | * @par 18 | * 19 | * Evaluates the cheirality constraint for each camera. 20 | * Cheirality is a very fancy word for the test if a 3d landmark could have been observed by the 21 | * camera. 22 | * In this simple version this is equal to the test if the 3d landmark is behin the image plane or 23 | * not 24 | */ 25 | class LandmarkRejectionSchemeCheirality : public LandmarkRejectionSchemeBase { 26 | public: // public classes/enums/types etc... 27 | public: // attributes 28 | public: // public methods 29 | // default constructor 30 | LandmarkRejectionSchemeCheirality() { 31 | identifier = "cheirality"; 32 | } 33 | 34 | // default destructor 35 | virtual ~LandmarkRejectionSchemeCheirality() = default; 36 | 37 | // default move 38 | LandmarkRejectionSchemeCheirality(LandmarkRejectionSchemeCheirality&& other) = default; 39 | LandmarkRejectionSchemeCheirality& operator=(LandmarkRejectionSchemeCheirality&& other) = default; 40 | 41 | // default copy 42 | LandmarkRejectionSchemeCheirality(const LandmarkRejectionSchemeCheirality& other) = default; 43 | LandmarkRejectionSchemeCheirality& operator=(const LandmarkRejectionSchemeCheirality& other) = default; 44 | 45 | std::set getSelection(const LandmarkMap& landmarks, const KeyframeMap& keyframes) const override; 46 | 47 | ////////////////////////////////////////////////// 48 | /// \brief createConst, factory function for const pointer 49 | /// \return 50 | /// 51 | static LandmarkRejectionSchemeBase::ConstPtr createConst(); 52 | 53 | ////////////////////////////////////////////////// 54 | /// \brief create, factory function 55 | /// \return 56 | /// 57 | static LandmarkRejectionSchemeBase::Ptr create(); 58 | }; 59 | } 60 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/landmark_selection_scheme_dimension_plausibility.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "landmark_selection_scheme_base.hpp" 12 | 13 | namespace keyframe_bundle_adjustment { 14 | 15 | /** 16 | * @class Landmark selection scheme random 17 | * @par 18 | * 19 | * Concrete LandmarkSelectionScheme; select num_landmarks randomly 20 | */ 21 | class LandmarkRejectionSchemeDimensionPlausibility : public LandmarkRejectionSchemeBase { 22 | public: // public classes/enums/types etc... 23 | struct Params { 24 | double min_x{std::numeric_limits::min()}, max_x{std::numeric_limits::max()}; 25 | double min_y{std::numeric_limits::min()}, max_y{std::numeric_limits::max()}; 26 | double min_z{std::numeric_limits::min()}, max_z{std::numeric_limits::max()}; 27 | }; 28 | 29 | public: // public methods 30 | // default constructor 31 | LandmarkRejectionSchemeDimensionPlausibility(const Params& p) : params_(p) { 32 | identifier = "dimension plausibility"; 33 | } 34 | 35 | // default destructor 36 | virtual ~LandmarkRejectionSchemeDimensionPlausibility() = default; 37 | 38 | // default move 39 | LandmarkRejectionSchemeDimensionPlausibility(LandmarkRejectionSchemeDimensionPlausibility&& other) = default; 40 | LandmarkRejectionSchemeDimensionPlausibility& operator=(LandmarkRejectionSchemeDimensionPlausibility&& other) = 41 | default; 42 | 43 | // default copy 44 | LandmarkRejectionSchemeDimensionPlausibility(const LandmarkRejectionSchemeDimensionPlausibility& other) = default; 45 | LandmarkRejectionSchemeDimensionPlausibility& operator=(const LandmarkRejectionSchemeDimensionPlausibility& other) = 46 | default; 47 | 48 | std::set getSelection(const LandmarkMap& landmarks, const KeyframeMap& keyframes) const override { 49 | auto it = 50 | std::max_element(keyframes.cbegin(), keyframes.cend(), [](const auto& a, const auto& b) { return a < b; }); 51 | const auto& newest_kf = *it; 52 | 53 | std::set out; 54 | for (const auto& el : landmarks) { 55 | Eigen::Vector3d p = 56 | newest_kf.second->getEigenPose() * Eigen::Map(el.second->pos.data()); 57 | if (p[0] > params_.min_x && p[0] < params_.max_x && p[1] > params_.min_y && p[1] < params_.max_y && 58 | p[2] > params_.min_z && p[2] < params_.max_z) { 59 | out.insert(el.first); 60 | } 61 | } 62 | 63 | return out; 64 | } 65 | 66 | ////////////////////////////////////////////////// 67 | /// \brief createConst, factory function for const pointer 68 | /// \return 69 | /// 70 | static ConstPtr createConst(const Params& p) { 71 | return LandmarkRejectionSchemeDimensionPlausibility::ConstPtr( 72 | new LandmarkRejectionSchemeDimensionPlausibility(p)); 73 | } 74 | 75 | ////////////////////////////////////////////////// 76 | /// \brief create, factory function 77 | /// \return 78 | /// 79 | static Ptr create(const Params& p) { 80 | return LandmarkRejectionSchemeDimensionPlausibility::Ptr(new LandmarkRejectionSchemeDimensionPlausibility(p)); 81 | } 82 | 83 | 84 | private: // attributes 85 | Params params_; 86 | }; 87 | } 88 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/landmark_selection_scheme_helpers.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "definitions.hpp" 12 | #include "../keyframe.hpp" 13 | 14 | namespace keyframe_bundle_adjustment { 15 | 16 | namespace landmark_helpers { 17 | 18 | 19 | template 20 | void getMeasurementFromKf(iterator_type it, 21 | iterator_type end_iterator, 22 | LandmarkId lm_id, 23 | std::map& meas_per_cam, 24 | TimestampNSec& ts) { 25 | for (; it != end_iterator; it++) { 26 | for (const auto& cam : (*it)->cameras_) { 27 | const auto& cam_id = cam.first; 28 | if ((*it)->hasMeasurement(lm_id, cam_id)) { 29 | // calculate ray from measurement and poses 30 | auto map_meas = (*it)->getMeasurements(lm_id); 31 | assert(map_meas.size() == 1); 32 | auto first_measurement = map_meas.cbegin()->second; 33 | // const auto& cam_id = map_meas.cbegin()->first; 34 | // // get vewing ray no transformation needed, angle is independant of 35 | // // frame 36 | // const auto& cam = (*it)->cameras_.at(cam_id); 37 | // viewing_ray_per_cam[cam_id] = cam->getViewingRay(first_measurement); 38 | meas_per_cam[cam_id] = first_measurement; 39 | ts = (*it)->timestamp_; 40 | } 41 | } 42 | 43 | // only get first hit -> return 44 | if (!meas_per_cam.empty()) { 45 | break; 46 | } 47 | } 48 | } 49 | 50 | std::vector chooseNearLmIds(size_t max_num_lms, 51 | const std::vector& near_ids, 52 | const std::map& map_flow); 53 | 54 | std::vector chooseMiddleLmIds(size_t max_num, const std::vector& middle_ids); 55 | 56 | std::vector chooseFarLmIds(size_t max_num, 57 | const std::vector& ids_far, 58 | const std::map& keyframes); 59 | 60 | std::map calcFlow(const std::vector& landmarks, 61 | const std::vector& sorted_keyframes, 62 | bool use_mean = true); 63 | 64 | std::map calcFlow(const std::vector& landmarks, 65 | const std::map& keyframes, 66 | bool use_mean = true); 67 | } 68 | 69 | namespace keyframe_helpers { 70 | std::vector getSortedKeyframes(const std::map& keyframes); 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/landmark_selection_scheme_observability.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "landmark_categorization_interface.hpp" 12 | #include "landmark_selection_scheme_base.hpp" 13 | 14 | namespace keyframe_bundle_adjustment { 15 | 16 | /** 17 | * @class Landmark selection scheme observability 18 | * @par 19 | * 20 | * Concrete LandmarkSelectionScheme; select landmarks in function of their difference in angle. 21 | * All angles of the viewing rays of the measruements between keyframes will be calculated and put in 22 | * a histogram with 3 main bins. 23 | * 3 bins are called: near field, middle field, far field and describe the observability of the 24 | * measurements. Near features are good for translation, middle can be used for both rotation and 25 | * translation and far away features are important for good rotation estimation. 26 | * Take a fix number of measurements ineach of the bins, as defined in parameter struct. 27 | */ 28 | class LandmarkSparsificationSchemeObservability : public LandmarkSparsificationSchemeBase, 29 | public LandmarkCategorizatonInterface { 30 | public: // public classes/enums/types etc... 31 | struct BinParameters { 32 | unsigned int max_num_landmarks_near{300}; ///< maximum number of landmarks in near bin 33 | unsigned int max_num_landmarks_middle{300}; ///< maximum number of landmarks in middle bin 34 | unsigned int max_num_landmarks_far{300}; ///< maximum number of landmarks in far bin 35 | double bound_near_middle = 0.4; ///< bound between near field and middle field 36 | double bound_middle_far = 0.2; ///< bound between middle field and far field 37 | }; 38 | 39 | struct Parameters { 40 | Parameters() { 41 | histogram_cache_size = 500; 42 | } 43 | 44 | int histogram_cache_size; ///< cache size of accumulator with which the histogram 45 | /// is calculated. Bins are determind by those 46 | /// measruemetns afterwards they are only assigned. 47 | BinParameters bin_params_; 48 | }; 49 | 50 | public: // public methods 51 | // default constructor 52 | LandmarkSparsificationSchemeObservability(Parameters p) : params_(p) { 53 | identifier = "observability"; 54 | } 55 | 56 | // default destructor 57 | virtual ~LandmarkSparsificationSchemeObservability() = default; 58 | 59 | // default move 60 | LandmarkSparsificationSchemeObservability(LandmarkSparsificationSchemeObservability&& other) = default; 61 | LandmarkSparsificationSchemeObservability& operator=(LandmarkSparsificationSchemeObservability&& other) = default; 62 | 63 | // default copy 64 | LandmarkSparsificationSchemeObservability(const LandmarkSparsificationSchemeObservability& other) = default; 65 | LandmarkSparsificationSchemeObservability& operator=(const LandmarkSparsificationSchemeObservability& other) = 66 | default; 67 | 68 | std::set getSelection(const LandmarkMap& new_frame, const KeyframeMap& keyframes) const override; 69 | 70 | std::map getCategorizedSelection( 71 | const LandmarkMap& new_frame, const KeyframeMap& keyframes) const override; 72 | 73 | ////////////////////////////////////////////////// 74 | /// \brief createConst, factory function for const pointer 75 | /// \return 76 | /// 77 | static ConstPtr createConst(Parameters p); 78 | 79 | ////////////////////////////////////////////////// 80 | /// \brief create, factory function 81 | /// \return 82 | /// 83 | static Ptr create(Parameters p); 84 | 85 | 86 | public: // attributes 87 | Parameters params_; ///< paramters 88 | }; 89 | } 90 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/landmark_selection_scheme_random.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "landmark_selection_scheme_base.hpp" 12 | 13 | namespace keyframe_bundle_adjustment { 14 | 15 | /** 16 | * @class Landmark selection scheme random 17 | * @par 18 | * 19 | * Concrete LandmarkSelectionScheme; select num_landmarks randomly 20 | */ 21 | class LandmarkSparsificationSchemeRandom : public LandmarkSparsificationSchemeBase { 22 | public: // public classes/enums/types etc... 23 | public: // public methods 24 | // default constructor 25 | LandmarkSparsificationSchemeRandom(size_t num_landmarks) : num_landmarks_(num_landmarks) { 26 | identifier = "random"; 27 | } 28 | 29 | // default destructor 30 | virtual ~LandmarkSparsificationSchemeRandom() = default; 31 | 32 | // default move 33 | LandmarkSparsificationSchemeRandom(LandmarkSparsificationSchemeRandom&& other) = default; 34 | LandmarkSparsificationSchemeRandom& operator=(LandmarkSparsificationSchemeRandom&& other) = default; 35 | 36 | // default copy 37 | LandmarkSparsificationSchemeRandom(const LandmarkSparsificationSchemeRandom& other) = default; 38 | LandmarkSparsificationSchemeRandom& operator=(const LandmarkSparsificationSchemeRandom& other) = default; 39 | 40 | std::set getSelection(const LandmarkMap& landmarks, const KeyframeMap& keyframes) const override; 41 | 42 | ////////////////////////////////////////////////// 43 | /// \brief createConst, factory function for const pointer 44 | /// \return 45 | /// 46 | static ConstPtr createConst(size_t num_landmarks); 47 | 48 | ////////////////////////////////////////////////// 49 | /// \brief create, factory function 50 | /// \return 51 | /// 52 | static Ptr create(size_t num_landmarks); 53 | 54 | 55 | public: // attributes 56 | size_t num_landmarks_; 57 | }; 58 | } 59 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/landmark_selection_scheme_voxel.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "landmark_categorization_interface.hpp" 12 | #include "landmark_selection_scheme_base.hpp" 13 | 14 | namespace keyframe_bundle_adjustment { 15 | /** 16 | * @class LandmarkSelectionSchemeVoxel 17 | * @par 18 | * 19 | * Concrete LandmarkSelectionScheme; sparsify landmarks by running a voxelfilter 20 | * of landmark pointcloud. 21 | */ 22 | class LandmarkSparsificationSchemeVoxel : public LandmarkSparsificationSchemeBase, 23 | public LandmarkCategorizatonInterface { 24 | public: // Public classes/enums/types etc... 25 | struct Parameters { 26 | Parameters() { 27 | ; 28 | } 29 | std::array voxel_size_xyz{{1.0, 1.0, 0.5}}; ///< Voxelsize in x,y,z in meters 30 | std::array roi_far_xyz{{ 31 | 50., 50., 50.}}; ///< Roi around current position in hwich points will be considered 32 | std::array roi_middle_xyz{{ 33 | 25., 25., 25.}}; ///< Roi around current position in hwich points will be considered 34 | unsigned int max_num_landmarks_near{300}; ///< maximum number of landmarks in near bin 35 | unsigned int max_num_landmarks_middle{300}; ///< maximum number of landmarks in middle bin 36 | unsigned int max_num_landmarks_far{300}; ///< maximum number of landmarks in far bin 37 | }; 38 | 39 | public: // Attributes. 40 | public: // Public methods. 41 | // Default constructor. 42 | explicit LandmarkSparsificationSchemeVoxel(Parameters p) : params_(p) { 43 | identifier = "voxel"; 44 | } 45 | 46 | // Default destructor. 47 | ~LandmarkSparsificationSchemeVoxel() = default; 48 | 49 | // Default move. 50 | LandmarkSparsificationSchemeVoxel(LandmarkSparsificationSchemeVoxel&& other) = default; 51 | LandmarkSparsificationSchemeVoxel& operator=(LandmarkSparsificationSchemeVoxel&& other) = default; 52 | 53 | // Default copy. 54 | LandmarkSparsificationSchemeVoxel(const LandmarkSparsificationSchemeVoxel& other) = default; 55 | LandmarkSparsificationSchemeVoxel& operator=(const LandmarkSparsificationSchemeVoxel& other) = default; 56 | 57 | std::set getSelection(const LandmarkMap& new_frame, const KeyframeMap& keyframes) const override; 58 | 59 | std::map getCategorizedSelection( 60 | const LandmarkMap& new_frame, const KeyframeMap& keyframes) const override; 61 | 62 | ////////////////////////////////////////////////// 63 | /// \brief createConst, factory function for const pointer 64 | /// \return 65 | /// 66 | static ConstPtr createConst(Parameters p = Parameters()) { 67 | return LandmarkSparsificationSchemeBase::ConstPtr(new LandmarkSparsificationSchemeVoxel(p)); 68 | } 69 | 70 | ////////////////////////////////////////////////// 71 | /// \brief create, factory function 72 | /// \return 73 | /// 74 | static Ptr create(Parameters p = Parameters()) { 75 | return LandmarkSparsificationSchemeBase::Ptr(new LandmarkSparsificationSchemeVoxel(p)); 76 | } 77 | 78 | 79 | public: // attributes 80 | Parameters params_; ///< paramters 81 | }; 82 | } 83 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/motion_model_regularization.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include "definitions.hpp" 14 | 15 | namespace keyframe_bundle_adjustment { 16 | namespace regularization { 17 | 18 | namespace { 19 | namespace center_of_rotation { 20 | template 21 | T getDeltaY(const T& d_yaw, const T& d_x) { 22 | // Radius of rotation r=arclength/yaw, movement=[x,y]=[r*sin(yaw), r*(1-cos(yaw))], residual is hence 23 | // y_{measured}-y_{motion_model} with y_{motion_model}=r*(1-cos(yaw))=x/sin(yaw)*(1-cos(yaw)) 24 | if (ceres::abs(d_yaw - T(0.)) < T(1.e-6)) { 25 | return T(0.); 26 | } 27 | return d_x / ceres::sin(d_yaw) * (T(1.) - ceres::cos(d_yaw)); 28 | } 29 | } 30 | } 31 | 32 | class MotionModelRegularization { 33 | public: // public methods 34 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW // This is not needed since no eigen stuff is a member. 35 | // default constructor 36 | MotionModelRegularization() = default; 37 | 38 | template 39 | bool operator()(const T* const pose_keyframe1_origin, const T* const pose_keyframe0_origin, T* residual) const { 40 | using Pose = Eigen::Transform; 41 | Pose p_K0_O = convert(pose_keyframe0_origin); 42 | Pose p_K1_O = convert(pose_keyframe1_origin); 43 | 44 | Pose motion_K1_K0 = p_K1_O * p_K0_O.inverse(); 45 | 46 | // Eigen::eulerAngles is aorund moving axes however dynamic "ZYX" should be identical to static "XYZ" accoridng 47 | // to this 48 | // https://stackoverflow.com/questions/27508242/roll-pitch-and-yaw-from-rotation-matrix-with-eigen-library 49 | // Eigen::Matrix yrp = motion_K0_K1.rotation().eulerAngles(2, 1, 0); 50 | 51 | // Get yaw from quaternion. 52 | // https://stackoverflow.com/questions/5782658/extracting-yaw-from-a-quaternion 53 | Eigen::Quaternion q(motion_K1_K0.rotation()); 54 | q.x() = T(0.); 55 | q.y() = T(0.); 56 | q.normalize(); 57 | 58 | T sign = q.z() < T(0.) ? T(-1.) : T(1.); 59 | T yaw = sign * 2. * ceres::acos(q.w()); 60 | 61 | T d_y_motion_model = center_of_rotation::getDeltaY(yaw, motion_K1_K0.translation()[0]); 62 | 63 | if (std::is_same::value) { 64 | std::cout << "y motion_model=" << d_y_motion_model << std::endl; 65 | std::cout << "delta_motion translation=" << motion_K1_K0.translation()[0] << " " 66 | << motion_K1_K0.translation()[1] << std::endl; 67 | std::cout << "yaw=" << yaw << std::endl; 68 | } 69 | residual[0] = motion_K1_K0.translation()[1] - d_y_motion_model; 70 | residual[1] = motion_K1_K0.translation()[2] - 0.; 71 | 72 | return true; 73 | } 74 | 75 | static ceres::CostFunction* Create() { 76 | return (new ceres::AutoDiffCostFunction(new MotionModelRegularization())); 77 | } 78 | }; 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/internal/triangulator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016. All rights reserved. 3 | * Institute of Measurement and Control Systems 4 | * Karlsruhe Institute of Technology, Germany 5 | * 6 | * authors: 7 | * Johannes Graeter (johannes.graeter@kit.edu) 8 | * and others 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include "definitions.hpp" 27 | 28 | namespace keyframe_bundle_adjustment { 29 | /** 30 | * @class Triangulator 31 | * The reconstructed points are defined in the coordinate system in which poses are defined 32 | * 33 | */ 34 | template 35 | class Triangulator { 36 | public: // public classes/enums/types etc... 37 | using v3T = Eigen::Matrix; 38 | using t3T = Eigen::Transform; 39 | using PoseAndRay = std::pair, v3T>; // so that pose can be stored somewhere else 40 | 41 | using RayTracklet = std::vector>; 42 | 43 | public: // attributes 44 | public: // public methods 45 | /* 46 | * default constructor 47 | */ 48 | Triangulator() = default; 49 | 50 | ///@brief get landmark from rays and poses of 1 track, reference is poses_rays[0] 51 | v3T triangulate_rays(const std::vector& poses_rays) { 52 | assert(poses_rays.size() > 1); 53 | 54 | Eigen::Matrix sum_rrt; 55 | v3T rhs; 56 | sum_rrt.setZero(); 57 | rhs.setZero(); 58 | Eigen::Matrix id_3 = Eigen::Matrix::Identity(); 59 | 60 | for (const auto& p_r : poses_rays) { 61 | const auto& pose_origin_camera = *p_r.first; 62 | const auto& ray_camera = p_r.second; 63 | 64 | v3T r_trans = pose_origin_camera.rotation() * ray_camera; 65 | 66 | Eigen::Matrix cur_rrt = id_3 - r_trans * r_trans.transpose(); 67 | sum_rrt += cur_rrt; 68 | rhs += cur_rrt * pose_origin_camera.translation(); // for svp else: rhs+= cur_rrt * pose * p0 69 | } 70 | 71 | // this is the reconstructed point 72 | v3T p = sum_rrt.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(rhs); 73 | 74 | return p; 75 | } 76 | 77 | ///@brief wrapper for triangulation to pair poses and tracklets 78 | v3T process(const std::vector>& t, 79 | const std::map& map_poses) { 80 | std::vector poses_and_rays; 81 | poses_and_rays.reserve(t.size()); 82 | for (const auto& m : t) { 83 | const auto& cur_pose = map_poses.at(m.second); 84 | //@todo don't copy landmarks 85 | poses_and_rays.push_back(std::make_pair(std::make_shared(cur_pose), m.first)); 86 | } 87 | 88 | return triangulate_rays(poses_and_rays); 89 | } 90 | 91 | ///@brief triangulate all tracks 92 | std::map process( 93 | const std::map& tracklets, 94 | const std::map& map_poses) { 95 | std::map out; 96 | 97 | for (const auto& t : tracklets) { 98 | if (t.second.size() > 1) { 99 | auto p = process(t.second, map_poses); 100 | out[t.first] = p; 101 | } 102 | } 103 | 104 | return out; 105 | } 106 | }; 107 | } 108 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/keyframe_selection_schemes.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "internal/keyframe_rejection_scheme_flow.hpp" 12 | #include "internal/keyframe_schemes_base.hpp" 13 | #include "internal/keyframe_selection_scheme_pose.hpp" 14 | #include "internal/keyframe_sparsification_scheme_time.hpp" 15 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/keyframe_selector.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | // standard includes 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "keyframe.hpp" 25 | #include "keyframe_selection_schemes.hpp" 26 | 27 | 28 | namespace keyframe_bundle_adjustment { 29 | 30 | /** 31 | * @class KeyframeSelector 32 | * @par 33 | * 34 | * Keyframe selector which executes different selection schemes one after the other 35 | */ 36 | class KeyframeSelector { 37 | public: // public classes/enums/types etc... 38 | using Keyframes = std::set; 39 | 40 | public: // public methods 41 | // default constructor 42 | KeyframeSelector() = default; 43 | 44 | // default destructor 45 | virtual ~KeyframeSelector() = default; 46 | 47 | // default move 48 | KeyframeSelector(KeyframeSelector&& other) = default; 49 | KeyframeSelector& operator=(KeyframeSelector&& other) = default; 50 | 51 | // default copy 52 | KeyframeSelector(const KeyframeSelector& other) = default; 53 | KeyframeSelector& operator=(const KeyframeSelector& other) = default; 54 | 55 | 56 | /** 57 | * @brief addScheme, add scheme to be used, overloaded for selection rejection and 58 | * sparsificatino 59 | * @param scheme 60 | */ 61 | void addScheme(KeyframeSelectionSchemeBase::ConstPtr scheme); 62 | void addScheme(KeyframeRejectionSchemeBase::ConstPtr scheme); 63 | void addScheme(KeyframeSparsificationSchemeBase::ConstPtr scheme); 64 | 65 | /** 66 | * @brief select, select keyframes by comparing to last selected frames with SelectionSchemes 67 | * @param frames, reference to frames that shall be selected 68 | */ 69 | Keyframes select(const Keyframes& frames, std::map buffer_selected_frames); 70 | 71 | 72 | public: // attributes 73 | std::vector selection_schemes_; ///< schemes that are 74 | /// used for keyframe 75 | /// selection(frames 76 | /// that are needed 77 | /// for stability) 78 | std::vector 79 | rejection_schemes_; ///< schemes for rejection of keyframes for stability ///selection, (so 80 | std::vector 81 | sparsification_schemes_; ///< schemes for sparsification of keyframes to reduce amount of 82 | /// redundant information ///selection, (so 83 | }; 84 | } 85 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/include/keyframe_bundle_adjustment/landmark_selection_schemes.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include "internal/landmark_selection_scheme_add_depth.hpp" 12 | #include "internal/landmark_selection_scheme_base.hpp" 13 | #include "internal/landmark_selection_scheme_cheirality.hpp" 14 | #include "internal/landmark_selection_scheme_observability.hpp" 15 | #include "internal/landmark_selection_scheme_random.hpp" 16 | #include "internal/landmark_selection_scheme_voxel.hpp" 17 | #include "internal/landmark_selection_scheme_dimension_plausibility.hpp" 18 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | keyframe_bundle_adjustment 4 | 0.0.0 5 | ToDo: Edit package description 6 | 7 | contact-the-maintainer 8 | Johannes Graeter 9 | Johannes Graeter 10 | https://gitlab.mrt.uni-karlsruhe.de/graeter/keyframe_bundle_adjustment.git 11 | 12 | catkin 13 | mrt_cmake_modules 14 | gtest 15 | 29 | eigen 30 | mrt_ceres 31 | mrt_pcl 32 | mrt_opencv 33 | matches_msg_types 34 | robust_optimization 35 | boost 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/src/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/keyframe_bundle_adjustment/src/.gitignore -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/src/definitions.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | namespace keyframe_bundle_adjustment { 3 | 4 | Landmark::Landmark() { 5 | ; 6 | } 7 | 8 | Landmark::Landmark(const Eigen::Vector3d& p, bool has_depth) : has_measured_depth(has_depth) { 9 | pos[0] = p[0]; 10 | pos[1] = p[1]; 11 | pos[2] = p[2]; 12 | } 13 | 14 | Pose convert(EigenPose p) { 15 | Eigen::Quaterniond q(p.rotation()); 16 | // why does q.normalize() not work? 17 | Pose pose; 18 | pose[0] = q.w(); 19 | pose[1] = q.x(); 20 | pose[2] = q.y(); 21 | pose[3] = q.z(); 22 | 23 | pose[4] = p.translation()[0]; 24 | pose[5] = p.translation()[1]; 25 | pose[6] = p.translation()[2]; 26 | 27 | return pose; 28 | } 29 | 30 | Camera::Camera(double f, const Eigen::Vector2d& pp, const EigenPose& pose_cam_veh) 31 | : focal_length(f), principal_point(pp) { 32 | pose_camera_vehicle = convert(pose_cam_veh); 33 | intrin_inv = getIntrinsicMatrix().inverse(); 34 | } 35 | 36 | Eigen::Matrix3d Camera::getIntrinsicMatrix() const { 37 | Eigen::Matrix3d intrin; 38 | intrin << focal_length, 0., principal_point[0], 0., focal_length, principal_point[1], 0., 0., 1.; 39 | return intrin; 40 | } 41 | 42 | EigenPose Camera::getEigenPose() const { 43 | return convert(pose_camera_vehicle); 44 | } 45 | 46 | Eigen::Vector3d Camera::getViewingRay(const Measurement& m) { 47 | Eigen::Vector3d meas_hom{m.u, m.v, 1.}; 48 | 49 | meas_hom = intrin_inv * meas_hom; 50 | meas_hom.normalize(); 51 | 52 | return meas_hom; 53 | } 54 | 55 | double calcRotRoccMetric(const Eigen::Isometry3d& transform_1_0, 56 | const Eigen::Matrix3d& intrinsics, 57 | const Eigen::Vector3d& lm_0, 58 | const Eigen::Vector2d& measurement_1) { 59 | // undefined for translation=0. 60 | if (transform_1_0.translation().norm() < 0.0001) { 61 | return 0.; 62 | } 63 | Eigen::Vector2d reproj_error_vec = measurement_1 - reproject(transform_1_0, intrinsics, lm_0); 64 | Eigen::Vector2d rot_error_vec = measurement_1 - reproject(transform_1_0.rotation(), intrinsics, lm_0); 65 | return reproj_error_vec.norm() / std::max(rot_error_vec.norm(), 1e-10); 66 | } 67 | 68 | TimestampSec convert(const TimestampNSec& ts) { 69 | return static_cast(ts * 1e-09); 70 | } 71 | 72 | TimestampNSec convert(const TimestampSec& ts) { 73 | return static_cast(ts * 1e09); 74 | } 75 | 76 | double getCost(const std::map>& ids, ceres::Problem& problem) { 77 | std::vector v; 78 | v.reserve(ids.size()); 79 | for (const auto& el : ids) { 80 | v.push_back(el.first); 81 | } 82 | return getCost(v, problem); 83 | } 84 | 85 | double getCost(const std::vector& ids, ceres::Problem& problem) { 86 | // If residual ids in EvaluateOptoin is empty, total cost is returned, this is not what we intend to do. 87 | if (ids.size() == 0) { 88 | std::cout << "no ids" << std::endl; 89 | return 0.; 90 | } 91 | double cost = 0.; 92 | ceres::Problem::EvaluateOptions opt; 93 | opt.residual_blocks = ids; 94 | problem.Evaluate(opt, &cost, NULL, NULL, NULL); 95 | return cost; 96 | } 97 | 98 | Eigen::Matrix convertMeasurementToRay(const Eigen::Matrix3d& intrin_inv, const Measurement& m) { 99 | Eigen::Matrix ray = 100 | (intrin_inv * Eigen::Vector3d(static_cast(m.u), static_cast(m.v), 1.)).normalized(); 101 | return ray; 102 | } 103 | 104 | double calcQuaternionDiff(const Pose& p0, const Pose& p1) { 105 | Eigen::Quaterniond q0(p0[0], p0[1], p0[2], p0[3]); 106 | Eigen::Quaterniond q1(p1[0], p1[1], p1[2], p1[3]); 107 | 108 | Eigen::Quaterniond q10 = q1.inverse() * q0; 109 | Eigen::AngleAxisd a(q10); 110 | return a.angle(); 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/src/keyframe_rejection_scheme_flow.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #include "internal/keyframe_rejection_scheme_flow.hpp" 10 | 11 | namespace keyframe_bundle_adjustment { 12 | KeyframeRejectionSchemeFlow::KeyframeRejectionSchemeFlow(double min_median_flow) 13 | : min_median_flow_squared_(min_median_flow * min_median_flow) { 14 | ; 15 | } 16 | 17 | bool KeyframeRejectionSchemeFlow::isUsable(const Keyframe::Ptr& new_frame, 18 | const std::map& last_frames) const { 19 | // nothing to test return true 20 | if (last_frames.size() == 0) { 21 | return true; 22 | } 23 | // no measurements, don't use 24 | if (new_frame->measurements_.size() == 0) { 25 | return false; 26 | } 27 | // get measurements of last keyframe 28 | auto it = std::max_element(last_frames.cbegin(), last_frames.cend(), [](const auto& a, const auto& b) { 29 | return a.second->timestamp_ < b.second->timestamp_; 30 | }); 31 | const Keyframe::Ptr& last_keyframe = it->second; 32 | 33 | // calculate flow for all cameras 34 | std::vector flow_squared; 35 | flow_squared.reserve(new_frame->measurements_.size()); 36 | 37 | for (const auto& m : new_frame->measurements_) { 38 | const auto& lm_id = m.first; 39 | const auto& map_cam_meas = m.second; 40 | 41 | for (const auto& cam_id_meas : map_cam_meas) { 42 | if (last_keyframe->hasMeasurement(lm_id, cam_id_meas.first)) { 43 | const Measurement& last_meas = last_keyframe->getMeasurement(lm_id, cam_id_meas.first); 44 | Eigen::Vector2d diff = cam_id_meas.second.toEigen2d() - last_meas.toEigen2d(); 45 | flow_squared.push_back(diff.squaredNorm()); 46 | } 47 | } 48 | } 49 | 50 | // // get median flow 51 | // std::nth_element(flow_squared.begin(), 52 | // flow_squared.begin() + flow_squared.size() / 2, 53 | // flow_squared.end(), 54 | // [](const auto& a, const auto& b) { return a < b; }); 55 | 56 | // double median_flow = flow_squared[flow_squared.size() / 2]; 57 | 58 | // get mean flow 59 | double sum = 0.; 60 | for (const auto& el : flow_squared) { 61 | sum += std::sqrt(el); 62 | } 63 | sum /= static_cast(flow_squared.size()); 64 | // This is not median but mean, 65 | ///@todo change names. 66 | double median_flow = sum * sum; 67 | 68 | if (median_flow < min_median_flow_squared_) { 69 | std::cout << "---- Not enough flow, reject frame." << std::endl; 70 | } 71 | 72 | // accept if median flow is bigger than threshold 73 | return median_flow > min_median_flow_squared_; 74 | } 75 | 76 | KeyframeRejectionSchemeBase::ConstPtr KeyframeRejectionSchemeFlow::createConst(double min_median_flow) { 77 | return KeyframeRejectionSchemeBase::ConstPtr(new KeyframeRejectionSchemeFlow(min_median_flow)); 78 | } 79 | 80 | KeyframeRejectionSchemeBase::Ptr KeyframeRejectionSchemeFlow::create(double min_median_flow) { 81 | return KeyframeRejectionSchemeBase::Ptr(new KeyframeRejectionSchemeFlow(min_median_flow)); 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/src/keyframe_selection_scheme_pose.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #include "internal/keyframe_selection_scheme_pose.hpp" 10 | 11 | namespace keyframe_bundle_adjustment { 12 | 13 | KeyframeSelectionSchemePose::KeyframeSelectionSchemePose(double critical_quaternion_difference) 14 | : critical_quaternion_diff_(critical_quaternion_difference) { 15 | ; 16 | } 17 | 18 | bool KeyframeSelectionSchemePose::isUsable(const Keyframe::Ptr& new_frame, 19 | const std::map& last_frames) const { 20 | 21 | // nothing to test return false, otherwise empty ones would always be taken 22 | if (last_frames.size() == 0) { 23 | return false; 24 | } 25 | // get frame with last timestamp 26 | auto it = std::max_element(last_frames.cbegin(), last_frames.cend(), [](const auto& a, const auto& b) { 27 | return a.second->timestamp_ < b.second->timestamp_; 28 | }); 29 | const Keyframe::Ptr& last_keyframe = it->second; 30 | 31 | // get quaternion distance 32 | double quat_diff = calcQuaternionDiff(new_frame->pose_, last_keyframe->pose_); 33 | std::cout << "quaternion diff=" << quat_diff << std::endl; 34 | 35 | // use if quaternion difference is bigger than threshold 36 | return quat_diff > critical_quaternion_diff_; 37 | } 38 | 39 | KeyframeSelectionSchemeBase::ConstPtr KeyframeSelectionSchemePose::createConst(double critical_quaternion_difference) { 40 | return KeyframeSelectionSchemeBase::ConstPtr(new KeyframeSelectionSchemePose(critical_quaternion_difference)); 41 | } 42 | 43 | KeyframeSelectionSchemeBase::Ptr KeyframeSelectionSchemePose::create(double critical_quaternion_difference) { 44 | return KeyframeSelectionSchemeBase::Ptr(new KeyframeSelectionSchemePose(critical_quaternion_difference)); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/src/keyframe_sparsification_scheme_time.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #include "internal/keyframe_sparsification_scheme_time.hpp" 10 | 11 | namespace keyframe_bundle_adjustment { 12 | 13 | bool KeyframeSparsificationSchemeTime::isUsable(const Keyframe::Ptr& new_frame, 14 | const std::map& last_frames) const { 15 | // nothing to test -> return true 16 | if (last_frames.size() == 0) { 17 | return true; 18 | } 19 | 20 | // first have look at the time diff between last keyframes and new ones 21 | auto it = std::max_element(last_frames.cbegin(), last_frames.cend(), [](const auto& a, const auto& b) { 22 | return a.second->timestamp_ < b.second->timestamp_; 23 | }); 24 | const TimestampNSec max_ts = it->second->timestamp_; 25 | 26 | return (new_frame->timestamp_ - max_ts) > time_difference_nano_sec_; 27 | } 28 | 29 | KeyframeSparsificationSchemeBase::ConstPtr KeyframeSparsificationSchemeTime::createConst( 30 | double time_difference_nano_sec_) { 31 | return KeyframeSparsificationSchemeBase::ConstPtr(new KeyframeSparsificationSchemeTime(time_difference_nano_sec_)); 32 | } 33 | 34 | KeyframeSparsificationSchemeBase::Ptr KeyframeSparsificationSchemeTime::create(double time_difference_nano_sec_) { 35 | return KeyframeSparsificationSchemeBase::Ptr(new KeyframeSparsificationSchemeTime(time_difference_nano_sec_)); 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/src/landmark_selection_scheme_add_depth.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #include "internal/landmark_selection_scheme_add_depth.hpp" 10 | #include 11 | #include "internal/landmark_selection_scheme_helpers.hpp" 12 | 13 | namespace keyframe_bundle_adjustment { 14 | 15 | 16 | std::set LandmarkSelectionSchemeAddDepth::getSelection( 17 | const LandmarkSchemeBase::LandmarkMap& landmarks, const LandmarkSchemeBase::KeyframeMap& keyframes) const { 18 | // Define output. 19 | std::set out; 20 | 21 | // Put keyframes into vector and sort them. 22 | std::vector kf_ptrs_sorted_reverse = keyframe_helpers::getSortedKeyframes(keyframes); 23 | std::reverse(kf_ptrs_sorted_reverse.begin(), kf_ptrs_sorted_reverse.end()); 24 | 25 | 26 | // For every Frame index assure num of lms with depth. 27 | for (const auto& el : params_.params_per_keyframe) { 28 | FrameIndex ind; 29 | NumberLandmarks num_lms_to_add; 30 | Comparator comparator; 31 | Sorter sorter; 32 | std::tie(ind, num_lms_to_add, comparator, sorter) = el; 33 | 34 | if (ind > static_cast(kf_ptrs_sorted_reverse.size() - 1)) { 35 | continue; 36 | } 37 | 38 | // Get landmarks that fullfill the condition defined by comparator. 39 | std::vector ids; 40 | for (const auto& lm : kf_ptrs_sorted_reverse[ind]->measurements_) { 41 | if (landmarks.find(lm.first) != landmarks.cend() && comparator(landmarks.at(lm.first))) { 42 | ids.push_back(lm.first); 43 | } 44 | } 45 | 46 | // Get landmarks ids with cost for this keyframe. 47 | std::vector> ids_sort_val; 48 | ids_sort_val.reserve(ids.size()); 49 | for (const auto& id : ids) { 50 | Eigen::Vector3d local_lm = 51 | kf_ptrs_sorted_reverse[ind]->getEigenPose() * LmMap(landmarks.at(id)->pos.data()); 52 | std::vector cost; 53 | for (const auto& cam_meas : kf_ptrs_sorted_reverse[ind]->measurements_.at(id)) { 54 | cost.push_back(sorter(cam_meas.second, local_lm)); 55 | } 56 | auto max_it = 57 | std::max_element(cost.cbegin(), cost.cend(), [](const auto& a, const auto& b) { return a < b; }); 58 | ids_sort_val.push_back(std::make_pair(id, *max_it)); 59 | } 60 | 61 | // Add lms with lowest cost to selection. 62 | int incr = std::min(num_lms_to_add, static_cast(ids_sort_val.size())); 63 | std::partial_sort(ids_sort_val.begin(), 64 | std::next(ids_sort_val.begin(), incr), 65 | ids_sort_val.end(), 66 | [](const auto& a, const auto& b) { return a.second < b.second; }); 67 | 68 | // Insert them in selection. 69 | std::transform(ids_sort_val.begin(), 70 | std::next(ids_sort_val.begin(), incr), 71 | std::inserter(out, out.begin()), 72 | [](const auto& a) { return a.first; }); 73 | } 74 | 75 | return out; 76 | } 77 | 78 | LandmarkSelectionSchemeBase::ConstPtr LandmarkSelectionSchemeAddDepth::createConst( 79 | LandmarkSelectionSchemeAddDepth::Parameters p) { 80 | return LandmarkSelectionSchemeBase::ConstPtr(new LandmarkSelectionSchemeAddDepth(p)); 81 | } 82 | 83 | LandmarkSelectionSchemeBase::Ptr LandmarkSelectionSchemeAddDepth::create( 84 | LandmarkSelectionSchemeAddDepth::Parameters p) { 85 | return LandmarkSelectionSchemeBase::Ptr(new LandmarkSelectionSchemeAddDepth(p)); 86 | } 87 | } 88 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/src/landmark_selection_scheme_cheirality.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #include "internal/landmark_selection_scheme_cheirality.hpp" 10 | #include 11 | 12 | namespace keyframe_bundle_adjustment { 13 | 14 | namespace { 15 | /** 16 | * @brief is_landmark_cheiral, test if landmark fullfills cheirality constraint for all cameras on 17 | * all keyframes 18 | * @param keyframes 19 | * @param lm 20 | * @return 21 | */ 22 | bool is_landmark_cheiral(const LandmarkSchemeBase::KeyframeMap& keyframes, 23 | const std::pair& lm) { 24 | for (const auto& id_kf : keyframes) { 25 | const auto& kf = id_kf.second; 26 | if (kf->is_active_) { 27 | // test cheirality for every projected landmark in cams 28 | for (const auto& cam_lm : kf->getProjectedLandmarkPosition(lm)) { 29 | if (cam_lm.second.z() < 0.) { 30 | return false; 31 | } 32 | } 33 | } 34 | } 35 | return true; 36 | } 37 | } 38 | 39 | std::set LandmarkRejectionSchemeCheirality::getSelection( 40 | const LandmarkSchemeBase::LandmarkMap& landmarks, const LandmarkSchemeBase::KeyframeMap& keyframes) const { 41 | 42 | std::set out; 43 | auto start_time_cheriality = std::chrono::steady_clock::now(); 44 | 45 | // transform landmarks into frames where they were observed 46 | // if all landmarks are in front of the camera, select landmark 47 | for (const auto& lm_el : landmarks) { 48 | if (is_landmark_cheiral(keyframes, lm_el)) { 49 | out.insert(lm_el.first); 50 | } 51 | } 52 | 53 | std::cout << "Duration cheirality check=" 54 | << std::chrono::duration_cast(std::chrono::steady_clock::now() - 55 | start_time_cheriality) 56 | .count() 57 | << " ms" << std::endl; 58 | 59 | return out; 60 | } 61 | 62 | LandmarkRejectionSchemeBase::ConstPtr LandmarkRejectionSchemeCheirality::createConst() { 63 | return LandmarkRejectionSchemeBase::ConstPtr(new LandmarkRejectionSchemeCheirality()); 64 | } 65 | 66 | LandmarkRejectionSchemeBase::Ptr LandmarkRejectionSchemeCheirality::create() { 67 | 68 | return LandmarkRejectionSchemeBase::Ptr(new LandmarkRejectionSchemeCheirality()); 69 | } 70 | } 71 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment/src/landmark_selection_scheme_random.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #include "internal/landmark_selection_scheme_random.hpp" 10 | 11 | namespace keyframe_bundle_adjustment { 12 | 13 | std::set LandmarkSparsificationSchemeRandom::getSelection(const LandmarkMap& landmarks, 14 | const KeyframeMap& keyframes) const { 15 | 16 | // copy indices from landmarks 17 | std::vector lm_ids; 18 | lm_ids.resize(landmarks.size()); 19 | std::transform(landmarks.cbegin(), landmarks.cend(), lm_ids.begin(), [](const auto& a) { return a.first; }); 20 | 21 | // shuffle them and take first num_landmarks 22 | std::random_shuffle(lm_ids.begin(), lm_ids.end()); 23 | 24 | std::set selected_landmark_ids; 25 | for (auto it = lm_ids.cbegin(); it != lm_ids.cend() && it != lm_ids.cbegin() + num_landmarks_; it++) { 26 | selected_landmark_ids.insert(*it); 27 | } 28 | 29 | return selected_landmark_ids; 30 | } 31 | 32 | 33 | LandmarkSparsificationSchemeBase::ConstPtr LandmarkSparsificationSchemeRandom::createConst(size_t num_landmarks) { 34 | 35 | return LandmarkSparsificationSchemeBase::ConstPtr(new LandmarkSparsificationSchemeRandom(num_landmarks)); 36 | } 37 | 38 | LandmarkSparsificationSchemeBase::Ptr LandmarkSparsificationSchemeRandom::create(size_t num_landmarks) { 39 | 40 | return LandmarkSparsificationSchemeBase::Ptr(new LandmarkSparsificationSchemeRandom(num_landmarks)); 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MRT_PKG_VERSION 2.2) 2 | # Modify only if you know what you are doing! 3 | cmake_minimum_required(VERSION 2.8.12) 4 | project(keyframe_bundle_adjustment_ros_tool) 5 | 6 | ################### 7 | ## find packages ## 8 | ################### 9 | find_package(mrt_cmake_modules REQUIRED) 10 | set(MRT_ARCH "None") 11 | include(UseMrtStdCompilerFlags) 12 | include(UseMrtAutoTarget) 13 | 14 | include(GatherDeps) 15 | # Remove libs which cannot be found automatically 16 | #list(REMOVE_ITEM DEPENDEND_PACKAGES ...) 17 | find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES}) 18 | 19 | # Manually resolve removed dependend packages 20 | #find_package(...) 21 | 22 | # Mark files or folders for display in IDEs 23 | mrt_add_to_ide(README.md .gitlab-ci.yml) 24 | 25 | ######################## 26 | ## add python modules ## 27 | ######################## 28 | # This adds a python module if located under src/{PROJECT_NAME) 29 | mrt_python_module_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | # Add message, service and action files 36 | glob_ros_files(add_message_files msg) 37 | glob_ros_files(add_service_files srv) 38 | glob_ros_files(add_action_files action) 39 | 40 | # Generate added messages and services with any dependencies listed here 41 | if (ROS_GENERATE_MESSAGES) 42 | generate_messages( 43 | DEPENDENCIES 44 | #add dependencies here 45 | #std_msgs 46 | ) 47 | endif() 48 | 49 | # Generate dynamic reconfigure options 50 | file(GLOB PARAMS_FILES RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "cfg/*.params" "cfg/*.cfg" "cfg/*.mrtcfg" "cfg/*.rosif") 51 | if (PARAMS_FILES) 52 | generate_ros_parameter_files(${PARAMS_FILES}) 53 | generate_ros_interface_files(${PARAMS_FILES}) 54 | endif () 55 | 56 | ################################### 57 | ## Eigen 58 | #add_definitions(-DEIGEN_DONT_VECTORIZE -DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT) 59 | 60 | 61 | ################################### 62 | ## catkin specific configuration ## 63 | ################################### 64 | catkin_package( 65 | ) 66 | 67 | ########### 68 | ## Build ## 69 | ########### 70 | # Add include and library directories 71 | include_directories( 72 | ${mrt_INCLUDE_DIRS} 73 | ${catkin_INCLUDE_DIRS} 74 | ) 75 | 76 | link_directories( 77 | ${mrt_LIBRARY_DIRS} 78 | ) 79 | 80 | glob_folders(SRC_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/src") 81 | if (SRC_DIRECTORIES) 82 | # Found subfolders, add executable for each subfolder 83 | foreach(SRC_DIR ${SRC_DIRECTORIES}) 84 | mrt_add_node_and_nodelet(${SRC_DIR} FOLDER "src/${SRC_DIR}") 85 | endforeach() 86 | else() 87 | # No subfolder found, add executable and python modules for src folder 88 | mrt_add_node_and_nodelet(${PROJECT_NAME} FOLDER "src") 89 | endif() 90 | 91 | ############# 92 | ## Install ## 93 | ############# 94 | # Install all targets, headers by default and scripts and other files if specified (folders or files) 95 | mrt_install(PROGRAMS scripts FILES launch rviz maps res data nodelet_plugins.xml plugin_description.xml) 96 | 97 | ############# 98 | ## Testing ## 99 | ############# 100 | # Add test targets for cpp and python tests 101 | if (CATKIN_ENABLE_TESTING) 102 | # Include header in src folder via src//.h 103 | include_directories("src") 104 | mrt_add_ros_tests(test) 105 | mrt_add_nosetests(test) 106 | endif() 107 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/README.md: -------------------------------------------------------------------------------- 1 | # Keyframe bundle adjustment ros tool 2 | 3 | * This tool uses keyframe_bundle_adjustment to make postprocessing for visual odometry 4 | * In contrast to existing tools such as orbslam it is meant ot be very modular so you can try different prior estimation methods or scale estimation yourself 5 | * Nodes: 6 | * Mono standalone: input: tracklets from 1 camera; use 5 point as prior and do pure mono odometry without scale 7 | * Mono with motion prior: input: tracklets from 1 camera, scaled motion constraint; use motion constraint as prior and smooth trajectory, scale is not fixed but regularized in whole window 8 | * Multicam with motion prior: input: tracklets from multi camera, scaled motion constraint; smooth with multiple cameras 9 | * Mono/Mutlicam with lidar: input: tracklets with depth from 1/multi camera; use depth from lidar to estimate scale (Alex) 10 | 11 | ## Installation 12 | 13 | standard 14 | 15 | ## Usage 16 | 17 | ## History 18 | DONE: 19 | * Mono standalone 20 | 21 | TODO: 22 | 23 | * Mono with lidar (15.1.2018) 24 | * Mono with motion (due 24.11.) 25 | * Multicam with motion prior (due ????) 26 | * Multi with lidar (due ????) 27 | 28 | ## Credits 29 | 30 | * Johannes Gräter 31 | * Alexander Wilczynski (mono lidar) 32 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/launch/mono_lidar_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/launch/mono_lidar_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/launch/mono_standalone_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/launch/mono_standalone_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/launch/params/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/keyframe_bundle_adjustment_ros_tool/launch/params/.gitignore -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/launch/params/mono_lidar_parameters.yaml: -------------------------------------------------------------------------------- 1 | # Use this file to specify parameters, that do not have default values. 2 | # YAML Files can also be helpful if you want to save parameters for different use-cases. 3 | # In any way: Default values should go into the .if file! 4 | # 5 | # To generate a yaml file, use "rosrun rosinterface_handler generate_yaml /path/to/MonoLidar.if" 6 | # 7 | # Use 'key: value' pairs, e.g. 8 | # string: 'foo' 9 | # integer: 1234 10 | # float: 1234.5 11 | # boolean: true 12 | # vector: [1.0, 2.0, 3.4] 13 | # map: {"a": "b", "c": "d"} 14 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/launch/params/mono_standalone_parameters.yaml: -------------------------------------------------------------------------------- 1 | # Use this file to specify parameters, that do not have default values. 2 | # YAML Files can also be helpful if you want to save parameters for different use-cases. 3 | # In any way: Default values should go into the .if file! 4 | # 5 | # To generate a yaml file, use "rosrun rosinterface_handler generate_yaml /path/to/MonoStandalone.if" 6 | # 7 | # Use 'key: value' pairs, e.g. 8 | # string: 'foo' 9 | # integer: 1234 10 | # float: 1234.5 11 | # boolean: true 12 | # vector: [1.0, 2.0, 3.4] 13 | # map: {"a": "b", "c": "d"} 14 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/package.xml: -------------------------------------------------------------------------------- 1 | 2 | keyframe_bundle_adjustment_ros_tool 3 | 0.0.0 4 | ToDo: Edit package description 5 | 6 | contact-the-maintainer 7 | Johannes Graeter 8 | Johannes Graeter 9 | https://gitlab.mrt.uni-karlsruhe.de/MRT/keyframe_bundle_adjustment_ros_tool.git 10 | 11 | catkin 12 | mrt_cmake_modules 13 | rosinterface_handler 14 | message_generation 15 | message_runtime 16 | gtest 17 | rostest 18 | 19 | roscpp 20 | 21 | roslib 22 | nodelet 23 | 24 | keyframe_bundle_adjustment 25 | viso_feature_tracking_ros_tool 26 | 27 | matches_msg_ros 28 | matches_msg_depth_ros 29 | matches_msg_conversions_ros 30 | sensor_msgs 31 | nav_msgs 32 | mrt_pcl 33 | pcl_ros 34 | 35 | image_geometry 36 | 37 | yaml-cpp 38 | 39 | dynamic_reconfigure 40 | message_filters 41 | tf2_ros 42 | tf2_geometry_msgs 43 | tf2_eigen 44 | diagnostic_updater 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/res/kitti_eval_script.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | import os 3 | 4 | 5 | def main(): 6 | # Paths of the destination paths, KITTI_EVAL PARAM_SET is used for parameter tuning. 7 | # If unset it will just evaluate to "" 8 | dst_prepath = "/home/graeter/kitti_eval_tune_params/kitti_results_mono_lidar$1_$2/" 9 | # mkdir if not available 10 | os.makedirs(dst_prepath) 11 | 12 | # dump params 13 | with open(dst_prepath + "/params.yaml", "w") as file: 14 | file.write("depth_loss: %f\nreprojection_loss: %f" % (args.depth_loss, args.reprojection_loss)) 15 | 16 | # Rosbags 17 | bagpath = "/media/graeter/data/odometry/kitti_bags/all_with_labels/" 18 | 19 | bagindex = [ 20 | "00", 21 | "01", 22 | "02", 23 | "03", 24 | "04", 25 | "05", 26 | "06", 27 | "07", 28 | "08", 29 | "09", 30 | "10", 31 | "11", 32 | "12", 33 | "13", 34 | "14", 35 | "15", 36 | "16", 37 | "17", 38 | "18", 39 | "19", 40 | "20", 41 | "21" 42 | ] 43 | 44 | # _____ Run evaluation for all bags _____ 45 | 46 | # source path 47 | os.system("source $HOME/workspaces/keyframe_ba/devel/setup.bash") 48 | 49 | # core 50 | processes = {} 51 | processes["core"] = subprocess.Popen(["roscore"]) 52 | print("Started roscore") 53 | 54 | for bag in bagindex: 55 | print("Start eval sequ %s" % bag) 56 | # _____ Run main processes _____ 57 | # start the odometry (rosnode) and close the newly created window after the process has been killed: 58 | comm_str = "roslaunch demo_keyframe_bundle_adjustment_meta kitti_standalone.launch | tee /tmp/log_$bag.txt" 59 | processes["main"] = subprocess.Popen(comm_str.split(" ")) 60 | time.sleep(4.0) 61 | 62 | # reconfigure dump path by dynamic reconfigure 63 | dump_path_name = dst_prepath + bag + ".txt" 64 | comm_str = "rosrun dynamic_reconfigure dynparam set /mono_lidar dump_path " + dump_path_name 65 | processes["dyn_set0"] = subprocess.Popen(comm_str.split(" ")) 66 | 67 | if args.depth_loss > 0.: 68 | print("set depth loss to %f" % args.depth_loss) 69 | comm_str = "rosrun dynamic_reconfigure dynparam set /mono_lidar robust_loss_depth_thres %f" % args.depth_loss) 70 | processes["dyn_set1"] = subprocess.Popen(comm_str.split(" ")) 71 | 72 | if args.reprojection_loss > 0.: 73 | print("set reprojection loss to %f" % args.reprojection_loss) 74 | comm_str = "rosrun dynamic_reconfigure dynparam set /mono_lidar robust_loss_reprojection_thres %f" % args.reprojection_loss) 75 | processes["dyn_set2"] = subprocess.Popen(comm_str.split(" ")) 76 | 77 | # dump tf stuff so that we can get all tf frames that we want afterwards 78 | bag_path_name = dst_prepath + bag + "_tf.bag" 79 | 80 | comm_str = "rosbag record /tf /tf_static /groundtruth_pose/pose /clock -o %s __name:=my_record" % ( 81 | bag_path_name) 82 | processes["record"] = subprocess.Popen(comm_str.split(" ")) 83 | 84 | # starte rosbag 85 | bagpath_full = bagpath + bag + ".bag" 86 | print("Start bag: %s" % bagpath_full 87 | 88 | # better run rhe rosbag sowly/safe to avoid the risk of frame skip (kitti evaluation failes) 89 | speed = 0.3 90 | comm_str = "rosbag play %s -r %f -d 6 --clock" % (bagpath_full, speed) 91 | processes["play"] = subprocess.Popen(comm_str.split(" ")) 92 | 93 | # Kill monolidar gracefully to write all 94 | os.system("rosnode kill /mono_lidar") 95 | # Kill rosbag record via ros. Only killing window will leave it in active state. 96 | os.system("rosnode kill /my_record") 97 | time.sleep(2.0) 98 | 99 | # Kill stuff 100 | for name, p in processes: 101 | p.kill() 102 | 103 | time.sleep(5.0) 104 | 105 | print("done with sequence %s" % bag) 106 | 107 | if __name__ == "__main__": 108 | main() 109 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/res/kitti_eval_script.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Paths of the destination paths, KITTI_EVAL PARAM_SET is used for parameter tuning. 4 | # If unset it will just evaluate to "" 5 | dst_prepath="/home/graeter/kitti_eval_tune_params/kitti_results_mono_lidar$1_$2_$3/" 6 | # mkdir if not available 7 | mkdir -p $dst_prepath 8 | 9 | # dump params 10 | echo "depth_loss: $1\nreprojection_loss: $2\nshrubbery_weight: $3" > "$dst_prepath/params.yaml" 11 | 12 | echo "" 13 | echo "" 14 | echo "Eval All KITTI Sequences" 15 | 16 | # Rosbags 17 | bagpath="/media/graeter/data/odometry/kitti_bags/all_with_labels/" 18 | 19 | bagindex[0]="00" 20 | bagindex[1]="01" 21 | bagindex[2]="02" 22 | bagindex[3]="03" 23 | bagindex[4]="04" 24 | bagindex[5]="05" 25 | bagindex[6]="06" 26 | bagindex[7]="07" 27 | bagindex[8]="08" 28 | bagindex[9]="09" 29 | bagindex[10]="10" 30 | bagindex[11]="11" 31 | bagindex[12]="12" 32 | bagindex[13]="13" 33 | bagindex[14]="14" 34 | bagindex[15]="15" 35 | bagindex[16]="16" 36 | bagindex[17]="17" 37 | bagindex[18]="18" 38 | bagindex[19]="19" 39 | bagindex[20]="20" 40 | bagindex[21]="21" 41 | 42 | # _____ Run evaluation for all bags _____ 43 | 44 | # source path for main library. 45 | source $HOME/workspaces/keyframe_ba/devel/setup.bash 46 | # newer version of rosbag play supports rate-control-topics, so build it yourself and source it. 47 | source $HOME/workspaces/rosbag_play/devel/setup.bash --extend 48 | 49 | tmux kill-server 50 | 51 | # core 52 | tmux new-session -d -s core "roscore" 53 | echo "Started roscore" 54 | 55 | for bag in ${bagindex[@]} 56 | do 57 | echo "Start eval sequ $bag" 58 | # _____ Run main processes _____ 59 | # export variable for tmux 60 | export bag 61 | sleep 2 62 | # start the odometry (rosnode) and close the newly created window after the process has been killed: 63 | tmux new-session -d -s my_main "roslaunch demo_keyframe_bundle_adjustment_meta kitti_standalone.launch | tee /tmp/log_$bag.txt" 64 | #tmux new-session -d -s my_main "roslaunch demo_keyframe_bundle_adjustment_meta kitti_mono_standalone.launch | tee /tmp/log_$bag.txt" 65 | sleep 4 66 | 67 | # reconfigure dump path by dynamic reconfigure 68 | dump_path_name=$dst_prepath$bag".txt" 69 | export dump_path_name 70 | tmux new-session -d -s dyn_set0 "rosrun dynamic_reconfigure dynparam set /mono_lidar dump_path $dump_path_name;sleep 2" 71 | 72 | if ! [ -z "$1" ] 73 | then 74 | echo "set depth_thres to $1" 75 | export $1 76 | tmux new-session -d -s dyn_set1 "rosrun dynamic_reconfigure dynparam set /mono_lidar robust_loss_depth_thres $1;sleep 1" 77 | fi 78 | if ! [ -z "$2" ] 79 | then 80 | echo "set reprojection_thres to $2" 81 | export $2 82 | tmux new-session -d -s dyn_set2 "rosrun dynamic_reconfigure dynparam set /mono_lidar robust_loss_reprojection_thres $2;sleep 1" 83 | fi 84 | if ! [ -z "$3" ] 85 | then 86 | echo "set shrubbery_weight to $3" 87 | export $3 88 | tmux new-session -d -s dyn_set1 "rosrun dynamic_reconfigure dynparam set /mono_lidar shrubbery_weight $3;sleep 1" 89 | fi 90 | 91 | # dump tf stuff so that we can get all tf frames that we want afterwards 92 | bag_path_name=$dst_prepath$bag"_tf.bag" 93 | export bag_path_name 94 | #tmux new-session -d -s record "rosbag record /tf /tf_static /clock /groundtruth_pose/pose /estimate/landmarks /estimate_prior/trajectory /estimate/complete_path /estimate/active_path /gt/trajectory -o $bag_path_name __name:=my_record" 95 | 96 | # starte rosbag 97 | bagpath_full=$bagpath$bag".bag" 98 | echo "Start bag: $bagpath_full" 99 | 100 | # better run rhe rosbag sowly/safe to avoid the risk of frame skip (kitti evaluation failes) 101 | rosrun rosbag play "$bagpath_full" --clock -r 0.2 -d 6 --rate-control-topic "/estimate/trajectory" --rate-control-max-delay 0.8 102 | #rosbag play "$bagpath_full" -r 0.1 -d 6 --clock 103 | 104 | # Kill monolidar gracefully to write all 105 | rosnode kill /mono_lidar 106 | # Kill rosbag record via ros. Only killing window will leave it in active state. 107 | rosnode kill /my_record 108 | sleep 2 109 | 110 | # Move groudnplane record to output path 111 | mv /tmp/gp.txt $dst_prepath$bag"_gp.txt" 112 | 113 | tmux kill-session -t dyn_set0 114 | tmux kill-session -t dyn_set1 115 | tmux kill-session -t dyn_set2 116 | tmux kill-session -t record 117 | tmux kill-session -t my_main 118 | 119 | sleep 5 120 | 121 | echo "done with sequence $bag" 122 | done 123 | 124 | # Kill core 125 | tmux kill-session -t core 126 | 127 | # call kitti evaluation node 128 | # /home/wilczynski/U/workspace/test2/devel/lib/kitti_devkit_tool/evaluate_odometry_all $dst_path_kitti_main 129 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/res/outlier_labels.yaml: -------------------------------------------------------------------------------- 1 | outlier_labels: [ 0, 1, 2, 3, 5, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, -1 ] 2 | shrubbery_labels: [ 21, 22, 23 ] 3 | labels: 4 | # id label 5 | 0: 'unlabeled' 6 | 1: 'ego vehicle' 7 | 2: 'rectification border' 8 | 3: 'out of roi' 9 | 4: 'static' 10 | 5: 'dynamic' 11 | 6: 'ground' 12 | 7: 'road' 13 | 8: 'sidewalk' 14 | 9: 'parking' 15 | 10: 'rail track' 16 | 11: 'building' 17 | 12: 'wall' 18 | 13: 'fence' 19 | 14: 'guard rail' 20 | 15: 'bridge' 21 | 16: 'tunnel' 22 | 17: 'pole' 23 | 18: 'polegroup' 24 | 19: 'traffic light' 25 | 20: 'traffic sign' 26 | 21: 'vegetation' 27 | 22: 'terrain' 28 | 23: 'sky' 29 | 24: 'person' 30 | 25: 'rider' 31 | 26: 'car' 32 | 27: 'truck' 33 | 28: 'bus' 34 | 29: 'caravan' 35 | 30: 'trailer' 36 | 31: 'train' 37 | 32: 'motorcycle' 38 | 33: 'bicycle' 39 | -1: 'license plate' 40 | 41 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/res/tune_parameters_kitti.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | def main(): 4 | depth_thres=[0.1,0.11,0.12,0.13,0.14,0.15,0.16,0.17,0.18,0.19] 5 | repr_thres=[1.0,1.1,1.2,1.3,1.4,1.5,1.6,1.7,1.8,1.8,2.0] 6 | #weights=[0.0,0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1.0] 7 | weights=[0.9] 8 | 9 | for d in depth_thres: 10 | for r in repr_thres: 11 | for w in weights: 12 | # Run script 13 | os.system("bash kitti_eval_script.sh %f %f %f"%(d,r,w)) 14 | 15 | if __name__=="__main__": 16 | main() 17 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/src/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/keyframe_bundle_adjustment_ros_tool/src/.gitignore -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/src/commons/color_by_index_hsv.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2014. All rights reserved. 3 | * Institute of Measurement and Control Systems 4 | * Karlsruhe Institute of Technology, Germany 5 | * 6 | * authors: 7 | * Johannes Graeter (johannes.graeter@kit.edu) 8 | * and others 9 | */ 10 | #ifndef COLORBYINDEX_HPP 11 | #define COLORBYINDEX_HPP 12 | 13 | #include 14 | 15 | namespace util_image { 16 | /// ///////////////////////////////////////////// 17 | /// @brief function to convert color in hsv to bgr 18 | /// @return color in bgr 19 | /// @par color in hsv 20 | /// 21 | /// uses the intern opencv color representation, so max s,v = 255 max(h)=180 (increase of one means 22 | /// increase of 2°) 23 | inline cv::Scalar hsv2bgr(cv::Scalar ColorHSV) { 24 | cv::Mat old_color(1, 1, CV_8UC3, ColorHSV); 25 | cv::Mat new_color(1, 1, CV_8UC3); 26 | cv::cvtColor(old_color, new_color, CV_HSV2BGR); 27 | return cv::Scalar(new_color.at(0, 0)); 28 | } 29 | inline cv::Scalar get_color(uint32_t ID, int NumColors) { 30 | if (ID == 0) { 31 | return cv::Scalar(123, 22, 234); 32 | } else { 33 | uint32_t ModID = ID - 1; 34 | ModID = ModID % NumColors; 35 | 36 | int dH = 180 / NumColors; 37 | int S = 200; 38 | int V = 200; 39 | 40 | return hsv2bgr(cv::Scalar(ModID * dH, S, V)); 41 | } 42 | } 43 | 44 | inline cv::Scalar get_color(uint32_t ID) { 45 | // seed generator with ID -> always same color 46 | cv::RNG rng(ID); 47 | // Generate number from uniform distribution 48 | return cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)); 49 | } 50 | } 51 | #endif // COLORBYINDEX_HPP 52 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/src/mono_lidar/mono_lidar.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | #include "keyframe_bundle_adjustment_ros_tool/MonoLidarInterface.h" 22 | 23 | // Forward declaration camera model; 24 | namespace image_geometry { 25 | class PinholeCameraModel; 26 | } 27 | 28 | namespace keyframe_bundle_adjustment_ros_tool { 29 | 30 | class MonoLidar { 31 | 32 | using Interface = MonoLidarInterface; 33 | using ReconfigureConfig = MonoLidarConfig; 34 | using ReconfigureServer = dynamic_reconfigure::Server; 35 | 36 | using TrackletsMsg = matches_msg_depth_ros::MatchesMsgWithOutlierFlag; 37 | using CameraInfoMsg = sensor_msgs::CameraInfo; 38 | 39 | using SyncPolicy = message_filters::sync_policies::ExactTime; 40 | using Synchronizer = message_filters::Synchronizer; 41 | 42 | public: 43 | MonoLidar(ros::NodeHandle, ros::NodeHandle); 44 | 45 | // Define destructor for writing down sequence with points for plotting. 46 | ~MonoLidar(); 47 | 48 | private: 49 | void setupDiagnostics(); 50 | void checkSensorStatus(diagnostic_updater::DiagnosticStatusWrapper&); 51 | 52 | 53 | ///@brief process the input from ros, execute whatever, publish it 54 | void callbackSubscriber(const TrackletsMsg::ConstPtr&, const CameraInfoMsg::ConstPtr& camera_info_msg); 55 | 56 | void reconfigureRequest(const ReconfigureConfig&, uint32_t); 57 | 58 | Interface interface_; 59 | ReconfigureServer reconfigure_server_; 60 | 61 | tf2_ros::Buffer tf_buffer_; 62 | tf2_ros::TransformListener tf_listener_; 63 | tf2_ros::TransformBroadcaster tf_broadcaster_; 64 | 65 | diagnostic_updater::Updater updater_; ///< Diagnostic updater 66 | // std::unique_ptr> 67 | // publisherDiagnosed_; 68 | diagnostic_msgs::DiagnosticStatus diagnostic_status_; ///< Current diagnostic status 69 | 70 | ///@brief sync subscribers 71 | std::unique_ptr sync_; 72 | 73 | /////////////////////////////////////// 74 | /// \brief bundle_adjuster_ptr_, class for doing the bundle adjustment 75 | /// 76 | keyframe_bundle_adjustment::BundleAdjusterKeyframes::Ptr bundle_adjuster_; 77 | 78 | ////////////////////////////////////////////////// 79 | /// \brief keyframe_selector_ptr_, selection for keyframes 80 | /// 81 | keyframe_bundle_adjustment::KeyframeSelector keyframe_selector_; 82 | 83 | /** 84 | * @brief maybeSendPoseTf, if tf frame ids are set in interface, pusblish last keyframe pose in 85 | * tf 86 | * @param timestamp, timestamp at which it shall be published 87 | * @param last_pose, pose toi be published 88 | */ 89 | void maybeSendPoseTf(ros::Time timestamp, Eigen::Isometry3d pose); 90 | 91 | /** 92 | * @brief getPrior, get prior from keyframe to origin (according to our convention). 93 | * @param model, camera model 94 | * @param tracklets 95 | * @return 96 | */ 97 | Eigen::Isometry3d getPrior(const image_geometry::PinholeCameraModel& model, 98 | const ros::Time& cur_ts_ros, 99 | const keyframe_bundle_adjustment::Tracklets& tracklets); 100 | 101 | private: // attributes 102 | Eigen::Isometry3d trf_camera_vehicle; ///< extrinsic calibraion of camera defined from vechicle 103 | /// frame to camera frame 104 | Eigen::Isometry3d last_pose_origin_camera; ///< Last pose 105 | Eigen::Isometry3d accumulated_motion; ///< Accumulated motion 106 | ros::Time last_ts_solved_{0.0}; ///< Last timestamp when bundle_adjuster_.solve was called 107 | }; 108 | } // namespace keyframe_bundle_adjustment_ros_tool 109 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/src/mono_lidar/mono_lidar_node.cpp: -------------------------------------------------------------------------------- 1 | #include "mono_lidar.hpp" 2 | 3 | int main(int argc, char* argv[]) { 4 | 5 | ros::init(argc, argv, "mono_lidar_node"); 6 | 7 | keyframe_bundle_adjustment_ros_tool::MonoLidar mono_lidar(ros::NodeHandle(), ros::NodeHandle("~")); 8 | 9 | ros::spin(); 10 | return EXIT_SUCCESS; 11 | } 12 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/src/mono_lidar/mono_lidar_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "mono_lidar.hpp" 6 | 7 | namespace keyframe_bundle_adjustment_ros_tool { 8 | 9 | class MonoLidarNodelet : public nodelet::Nodelet { 10 | 11 | inline void onInit() override { 12 | impl_ = std::make_unique(getNodeHandle(), getPrivateNodeHandle()); 13 | } 14 | std::unique_ptr impl_; 15 | }; 16 | } // namespace keyframe_bundle_adjustment_ros_tool 17 | 18 | PLUGINLIB_EXPORT_CLASS(keyframe_bundle_adjustment_ros_tool::MonoLidarNodelet, nodelet::Nodelet); 19 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/src/mono_standalone/mono_standalone.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include "keyframe_bundle_adjustment_ros_tool/MonoStandaloneInterface.h" 20 | 21 | // ////////////////////////////////// 22 | // Forward declarations 23 | // 24 | namespace keyframe_bundle_adjustment { 25 | class BundleAdjusterKeyframes; 26 | } 27 | namespace keyframe_bundle_adjustment { 28 | class KeyframeSelector; 29 | } 30 | 31 | namespace keyframe_bundle_adjustment_ros_tool { 32 | 33 | class MonoStandalone { 34 | 35 | using Interface = MonoStandaloneInterface; 36 | using ReconfigureConfig = MonoStandaloneConfig; 37 | using ReconfigureServer = dynamic_reconfigure::Server; 38 | 39 | using TrackletsMsg = matches_msg_depth_ros::MatchesMsgWithOutlierFlag; 40 | using CameraInfoMsg = sensor_msgs::CameraInfo; 41 | 42 | using ApproximateTime = message_filters::sync_policies::ApproximateTime; 43 | using Synchronizer = message_filters::Synchronizer; 44 | 45 | public: 46 | MonoStandalone(ros::NodeHandle, ros::NodeHandle); 47 | 48 | /** 49 | * @brief maybeSendPoseTf, if tf frame ids are set in interface, pusblish last keyframe pose in 50 | * tf 51 | * @param timestamp, timestamp at which it shall be published 52 | * @param last_pose, pose toi be published 53 | */ 54 | void maybeSendPoseTf(ros::Time timestamp, Eigen::Isometry3d pose); 55 | 56 | 57 | private: 58 | // ///////////////////////////////////////////////////// 59 | // ROS STUFF 60 | // 61 | void setupDiagnostics(); 62 | void checkSensorStatus(diagnostic_updater::DiagnosticStatusWrapper&); 63 | 64 | void callbackSubscriber(const TrackletsMsg::ConstPtr& tracklets_msg, 65 | const CameraInfoMsg::ConstPtr& camera_info_msg); 66 | void reconfigureRequest(const ReconfigureConfig&, uint32_t); 67 | 68 | Interface interface_; 69 | ReconfigureServer reconfigure_server_; 70 | 71 | tf2_ros::Buffer tf_buffer_; 72 | tf2_ros::TransformListener tf_listener_; 73 | tf2_ros::TransformBroadcaster tf_broadcaster_; 74 | std::unique_ptr sync_; 75 | 76 | diagnostic_updater::Updater updater_; ///< Diagnostic updater 77 | // std::unique_ptr> 78 | // publisherDiagnosed_; 79 | diagnostic_msgs::DiagnosticStatus diagnostic_status_; ///< Current diagnostic status 80 | 81 | /////////////////////////////////////// 82 | /// \brief bundle_adjuster_ptr_, class for doing the bundle adjustment 83 | /// 84 | keyframe_bundle_adjustment::BundleAdjusterKeyframes::Ptr bundle_adjuster_; 85 | 86 | ////////////////////////////////////////////////// 87 | /// \brief keyframe_selector_ptr_, selection for keyframes 88 | /// 89 | keyframe_bundle_adjustment::KeyframeSelector keyframe_selector_; 90 | 91 | // Eigen::Isometry3d motion_camera_t0_t1; ///< save prior, so that if 5 point fails, we still have a prior 92 | 93 | Eigen::Isometry3d trf_camera_vehicle; ///< extrinsic calibraion of camera defined from vechicle 94 | /// frame to camera frame 95 | /// 96 | double last_ts_solved_{-10000000000}; ///< Last timestamp when bundle_adjuster_.solve was called, negative, so ba is 97 | ///called quickly at init. 98 | }; 99 | } // namespace keyframe_bundle_adjustment_ros_tool 100 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/src/mono_standalone/mono_standalone_node.cpp: -------------------------------------------------------------------------------- 1 | #include "mono_standalone.hpp" 2 | 3 | int main(int argc, char* argv[]) { 4 | 5 | ros::init(argc, argv, "mono_standalone_node"); 6 | 7 | keyframe_bundle_adjustment_ros_tool::MonoStandalone mono_standalone(ros::NodeHandle(), ros::NodeHandle("~")); 8 | 9 | ros::spin(); 10 | return EXIT_SUCCESS; 11 | } 12 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/src/mono_standalone/mono_standalone_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "mono_standalone.hpp" 6 | 7 | namespace keyframe_bundle_adjustment_ros_tool { 8 | 9 | class MonoStandaloneNodelet : public nodelet::Nodelet { 10 | 11 | inline void onInit() override { 12 | impl_ = std::make_unique(getNodeHandle(), getPrivateNodeHandle()); 13 | } 14 | std::unique_ptr impl_; 15 | }; 16 | } // namespace keyframe_bundle_adjustment_ros_tool 17 | 18 | PLUGINLIB_EXPORT_CLASS(keyframe_bundle_adjustment_ros_tool::MonoStandaloneNodelet, nodelet::Nodelet); 19 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/test/mono_lidar_node.cpp: -------------------------------------------------------------------------------- 1 | // google test docs 2 | // wiki page: https://code.google.com/p/googletest/w/list 3 | // primer: https://code.google.com/p/googletest/wiki/V1_7_Primer 4 | // FAQ: https://code.google.com/p/googletest/wiki/FAQ 5 | // advanced guide: https://code.google.com/p/googletest/wiki/V1_7_AdvancedGuide 6 | // samples: https://code.google.com/p/googletest/wiki/V1_7_Samples 7 | // 8 | // List of some basic tests fuctions: 9 | // Fatal assertion Nonfatal assertion Verifies / Description 10 | //------------------------------------------------------------------------------------------------------------------------------------------------------- 11 | // ASSERT_EQ(expected, actual); EXPECT_EQ(expected, actual); expected == actual 12 | // ASSERT_NE(val1, val2); EXPECT_NE(val1, val2); val1 != val2 13 | // ASSERT_LT(val1, val2); EXPECT_LT(val1, val2); val1 < val2 14 | // ASSERT_LE(val1, val2); EXPECT_LE(val1, val2); val1 <= val2 15 | // ASSERT_GT(val1, val2); EXPECT_GT(val1, val2); val1 > val2 16 | // ASSERT_GE(val1, val2); EXPECT_GE(val1, val2); val1 >= val2 17 | // 18 | // ASSERT_FLOAT_EQ(expected, actual); EXPECT_FLOAT_EQ(expected, actual); the two float values 19 | // are almost equal (4 20 | // ULPs) 21 | // ASSERT_DOUBLE_EQ(expected, actual); EXPECT_DOUBLE_EQ(expected, actual); the two double values 22 | // are almost equal (4 23 | // ULPs) 24 | // ASSERT_NEAR(val1, val2, abs_error); EXPECT_NEAR(val1, val2, abs_error); the difference between 25 | // val1 and val2 26 | // doesn't exceed the given absolute error 27 | //======================================================================================================================================================= 28 | 29 | #include 30 | #include "gtest/gtest.h" 31 | 32 | // This module test should only test the ros functionality of any node (does it run, does it respond 33 | // correctly to 34 | // messages). 35 | // The actual functionality should be tested in the library that implements it. 36 | // Any test acts like a node and can receive and send messages to your nodes/nodelets to test their 37 | // behaviour 38 | // You can even control time by setting "use_sim_time" in the .test-file and advertising the /clock 39 | // topic. 40 | // Keep in mind that: 41 | // - your node needs some time to initialize and subscribe to your topics 42 | // - if you control /clock, nothing might happen until you send the first clock message 43 | // - it takes time until a node responds to your messages 44 | // - utils_testing_ros provides functionality to make your testing life easier 45 | 46 | 47 | int main(int argc, char** argv) { 48 | testing::InitGoogleTest(&argc, argv); 49 | ros::init(argc, argv, "mono_lidar_test"); 50 | // The async spinner lets you publish and receive messages during the tests, no need to call 51 | // spinOnce() 52 | ros::AsyncSpinner spinner(1); 53 | spinner.start(); 54 | int ret = RUN_ALL_TESTS(); 55 | ros::shutdown(); 56 | return ret; 57 | } 58 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/test/mono_lidar_node.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/test/mono_lidar_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // google test docs 2 | // wiki page: https://code.google.com/p/googletest/w/list 3 | // primer: https://code.google.com/p/googletest/wiki/V1_7_Primer 4 | // FAQ: https://code.google.com/p/googletest/wiki/FAQ 5 | // advanced guide: https://code.google.com/p/googletest/wiki/V1_7_AdvancedGuide 6 | // samples: https://code.google.com/p/googletest/wiki/V1_7_Samples 7 | // 8 | // List of some basic tests fuctions: 9 | // Fatal assertion Nonfatal assertion Verifies / Description 10 | //------------------------------------------------------------------------------------------------------------------------------------------------------- 11 | // ASSERT_EQ(expected, actual); EXPECT_EQ(expected, actual); expected == actual 12 | // ASSERT_NE(val1, val2); EXPECT_NE(val1, val2); val1 != val2 13 | // ASSERT_LT(val1, val2); EXPECT_LT(val1, val2); val1 < val2 14 | // ASSERT_LE(val1, val2); EXPECT_LE(val1, val2); val1 <= val2 15 | // ASSERT_GT(val1, val2); EXPECT_GT(val1, val2); val1 > val2 16 | // ASSERT_GE(val1, val2); EXPECT_GE(val1, val2); val1 >= val2 17 | // 18 | // ASSERT_FLOAT_EQ(expected, actual); EXPECT_FLOAT_EQ(expected, actual); the two float values are almost equal (4 19 | // ULPs) 20 | // ASSERT_DOUBLE_EQ(expected, actual); EXPECT_DOUBLE_EQ(expected, actual); the two double values are almost equal (4 21 | // ULPs) 22 | // ASSERT_NEAR(val1, val2, abs_error); EXPECT_NEAR(val1, val2, abs_error); the difference between val1 and val2 23 | // doesn't exceed the given absolute error 24 | //======================================================================================================================================================= 25 | 26 | #include 27 | #include "gtest/gtest.h" 28 | 29 | // Because node and nodelet usually offer the same functionality, the nodelet test is only here to test 30 | // whether the nodelet starts and stops correcly. All the unittests can go to the node test. 31 | 32 | TEST(keyframe_bundle_adjustment_ros_tool, mono_lidar_test_initialization) { 33 | auto init_delay = ros::NodeHandle("~").param("init_delay", 3.); 34 | ros::Duration(init_delay).sleep(); 35 | ASSERT_TRUE(ros::ok()) << "Ros crashed or the nodelet failed to initialize!"; 36 | } 37 | 38 | int main(int argc, char** argv) { 39 | testing::InitGoogleTest(&argc, argv); 40 | ros::init(argc, argv, "mono_lidar_nodelet_test"); 41 | // The async spinner lets you publish and receive messages during the tests, no need to call spinOnce() 42 | ros::AsyncSpinner spinner(1); 43 | spinner.start(); 44 | int ret = RUN_ALL_TESTS(); 45 | ros::shutdown(); 46 | return ret; 47 | } 48 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/test/mono_lidar_nodelet.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/test/mono_standalone_node.cpp: -------------------------------------------------------------------------------- 1 | // google test docs 2 | // wiki page: https://code.google.com/p/googletest/w/list 3 | // primer: https://code.google.com/p/googletest/wiki/V1_7_Primer 4 | // FAQ: https://code.google.com/p/googletest/wiki/FAQ 5 | // advanced guide: https://code.google.com/p/googletest/wiki/V1_7_AdvancedGuide 6 | // samples: https://code.google.com/p/googletest/wiki/V1_7_Samples 7 | // 8 | // List of some basic tests fuctions: 9 | // Fatal assertion Nonfatal assertion Verifies / Description 10 | //------------------------------------------------------------------------------------------------------------------------------------------------------- 11 | // ASSERT_EQ(expected, actual); EXPECT_EQ(expected, actual); expected == actual 12 | // ASSERT_NE(val1, val2); EXPECT_NE(val1, val2); val1 != val2 13 | // ASSERT_LT(val1, val2); EXPECT_LT(val1, val2); val1 < val2 14 | // ASSERT_LE(val1, val2); EXPECT_LE(val1, val2); val1 <= val2 15 | // ASSERT_GT(val1, val2); EXPECT_GT(val1, val2); val1 > val2 16 | // ASSERT_GE(val1, val2); EXPECT_GE(val1, val2); val1 >= val2 17 | // 18 | // ASSERT_FLOAT_EQ(expected, actual); EXPECT_FLOAT_EQ(expected, actual); the two float values are almost equal (4 19 | // ULPs) 20 | // ASSERT_DOUBLE_EQ(expected, actual); EXPECT_DOUBLE_EQ(expected, actual); the two double values are almost equal (4 21 | // ULPs) 22 | // ASSERT_NEAR(val1, val2, abs_error); EXPECT_NEAR(val1, val2, abs_error); the difference between val1 and val2 23 | // doesn't exceed the given absolute error 24 | //======================================================================================================================================================= 25 | 26 | #include 27 | #include "gtest/gtest.h" 28 | 29 | // This module test should only test the ros functionality of any node (does it run, does it respond correctly to 30 | // messages). 31 | // The actual functionality should be tested in the library that implements it. 32 | // Any test acts like a node and can receive and send messages to your nodes/nodelets to test their behaviour 33 | // You can even control time by setting "use_sim_time" in the .test-file and advertising the /clock topic. 34 | // Keep in mind that: 35 | // - your node needs some time to initialize and subscribe to your topics 36 | // - if you control /clock, nothing might happen until you send the first clock message 37 | // - it takes time until a node responds to your messages 38 | // - utils_testing_ros provides functionality to make your testing life easier 39 | 40 | // TEST(keyframe_bundle_adjustment_ros_tool, MonoStandaloneTest){ 41 | // //your tests go here 42 | //} 43 | 44 | int main(int argc, char** argv) { 45 | testing::InitGoogleTest(&argc, argv); 46 | ros::init(argc, argv, "mono_standalone_test"); 47 | // The async spinner lets you publish and receive messages during the tests, no need to call spinOnce() 48 | ros::AsyncSpinner spinner(1); 49 | spinner.start(); 50 | int ret = RUN_ALL_TESTS(); 51 | ros::shutdown(); 52 | return ret; 53 | } 54 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/test/mono_standalone_node.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/test/mono_standalone_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // google test docs 2 | // wiki page: https://code.google.com/p/googletest/w/list 3 | // primer: https://code.google.com/p/googletest/wiki/V1_7_Primer 4 | // FAQ: https://code.google.com/p/googletest/wiki/FAQ 5 | // advanced guide: https://code.google.com/p/googletest/wiki/V1_7_AdvancedGuide 6 | // samples: https://code.google.com/p/googletest/wiki/V1_7_Samples 7 | // 8 | // List of some basic tests fuctions: 9 | // Fatal assertion Nonfatal assertion Verifies / Description 10 | //------------------------------------------------------------------------------------------------------------------------------------------------------- 11 | // ASSERT_EQ(expected, actual); EXPECT_EQ(expected, actual); expected == actual 12 | // ASSERT_NE(val1, val2); EXPECT_NE(val1, val2); val1 != val2 13 | // ASSERT_LT(val1, val2); EXPECT_LT(val1, val2); val1 < val2 14 | // ASSERT_LE(val1, val2); EXPECT_LE(val1, val2); val1 <= val2 15 | // ASSERT_GT(val1, val2); EXPECT_GT(val1, val2); val1 > val2 16 | // ASSERT_GE(val1, val2); EXPECT_GE(val1, val2); val1 >= val2 17 | // 18 | // ASSERT_FLOAT_EQ(expected, actual); EXPECT_FLOAT_EQ(expected, actual); the two float values are almost equal (4 19 | // ULPs) 20 | // ASSERT_DOUBLE_EQ(expected, actual); EXPECT_DOUBLE_EQ(expected, actual); the two double values are almost equal (4 21 | // ULPs) 22 | // ASSERT_NEAR(val1, val2, abs_error); EXPECT_NEAR(val1, val2, abs_error); the difference between val1 and val2 23 | // doesn't exceed the given absolute error 24 | //======================================================================================================================================================= 25 | 26 | #include 27 | #include "gtest/gtest.h" 28 | 29 | // Because node and nodelet usually offer the same functionality, the nodelet test is only here to test 30 | // whether the nodelet starts and stops correcly. All the unittests can go to the node test. 31 | 32 | TEST(keyframe_bundle_adjustment_ros_tool, mono_standalone_test_initialization) { 33 | auto init_delay = ros::NodeHandle("~").param("init_delay", 3.); 34 | ros::Duration(init_delay).sleep(); 35 | ASSERT_TRUE(ros::ok()) << "Ros crashed or the nodelet failed to initialize!"; 36 | } 37 | 38 | int main(int argc, char** argv) { 39 | testing::InitGoogleTest(&argc, argv); 40 | ros::init(argc, argv, "mono_standalone_nodelet_test"); 41 | // The async spinner lets you publish and receive messages during the tests, no need to call spinOnce() 42 | ros::AsyncSpinner spinner(1); 43 | spinner.start(); 44 | int ret = RUN_ALL_TESTS(); 45 | ros::shutdown(); 46 | return ret; 47 | } 48 | -------------------------------------------------------------------------------- /keyframe_bundle_adjustment_ros_tool/test/mono_standalone_nodelet.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /limo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(limo) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() -------------------------------------------------------------------------------- /limo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | limo 4 | 1.6.0 5 | Meta-package for feature_tracking library. 6 | Johannes Gräter 7 | GPLv3 8 | http://github.com/johannes-graeter/limo 9 | Johannes Gräter 10 | catkin 11 | keyframe_bundle_adjustment 12 | keyframe_bundle_adjustment_ros_tool 13 | util_nodes_tf2_ros_tool 14 | matches_msg_types 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /matches_msg_types/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | setup.py 3 | *.pyc 4 | CMakeLists.txt.user 5 | 6 | -------------------------------------------------------------------------------- /matches_msg_types/.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | #version=1.2 2 | variables: 3 | GIT_STRATEGY: none 4 | # Cache build results between building and testing 5 | cache: 6 | key: "$CI_COMMIT_SHA" 7 | paths: 8 | - catkin_ws/ 9 | stages: 10 | - build 11 | - test 12 | - deps 13 | 14 | # Prepare workspace and checkout the code. This will be executed before every stage 15 | before_script: 16 | # Environment variablen setzen 17 | - export SHELL="/bin/bash" 18 | # Prepare workspace (in folder catkin_ws) 19 | - sudo apt-get update 20 | - mrt ci prepare $CI_PROJECT_NAME -c $CI_COMMIT_SHA 21 | - cd catkin_ws 22 | # add current branch name to branch management 23 | - mrt ws branches add $CI_COMMIT_REF_NAME 24 | 25 | build: 26 | stage: build 27 | script: 28 | # Build project and resolve deps at the same time in debug mode 29 | - mrt catkin build -rd --debug --default_yes --no-status $CI_PROJECT_NAME 30 | 31 | test: 32 | stage: test 33 | script: 34 | # Build code again, in case caching didn't work 35 | - mrt catkin build -rd --debug --default_yes --no-status $CI_PROJECT_NAME 36 | # Run tests 37 | - source devel_debug/setup.bash 38 | - mrt catkin run_tests --no-status $CI_PROJECT_NAME --no-deps 39 | # Summarize results 40 | - catkin_test_results --verbose build_debug/$CI_PROJECT_NAME 41 | 42 | deps: 43 | stage: deps 44 | only: 45 | # only execute if this is the master branch 46 | - master 47 | script: 48 | # test all dependencies of this package. Exclude packages with -e, include with -i. 49 | - mrt ci test_deps -f --no-status --release $CI_PROJECT_NAME 50 | 51 | -------------------------------------------------------------------------------- /matches_msg_types/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MRT_PKG_VERSION 2.2) 2 | # Modify only if you know what you are doing! 3 | cmake_minimum_required(VERSION 2.8.12) 4 | project(matches_msg_types) 5 | 6 | ################### 7 | ## find packages ## 8 | ################### 9 | find_package(mrt_cmake_modules REQUIRED) 10 | include(UseMrtStdCompilerFlags) 11 | include(UseMrtAutoTarget) 12 | 13 | include(GatherDeps) 14 | # Remove libs which cannot be found automatically 15 | #list(REMOVE_ITEM DEPENDEND_PACKAGES ...) 16 | find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES}) 17 | 18 | # Manually resolve removed dependend packages 19 | #find_package(...) 20 | 21 | # Mark files or folders for display in IDEs 22 | mrt_add_to_ide(README.md .gitlab-ci.yml) 23 | 24 | ######################## 25 | ## add python modules ## 26 | ######################## 27 | # This adds a python module if located under src/{PROJECT_NAME) 28 | mrt_python_module_setup() 29 | 30 | file(GLOB PROJECT_PYTHON_SOURCE_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "python_api/*.cpp") 31 | if (PROJECT_PYTHON_SOURCE_FILES_SRC) 32 | # Add a cpp-python api library. Make sure there are no name collisions with python modules in this project 33 | mrt_add_python_api( ${PROJECT_NAME} 34 | FILES ${PROJECT_PYTHON_SOURCE_FILES_SRC} 35 | ) 36 | endif() 37 | 38 | ############################ 39 | ## read source code files ## 40 | ############################ 41 | file(GLOB_RECURSE PROJECT_HEADER_FILES_INC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "include/*.h" "include/*.hpp") 42 | file(GLOB PROJECT_SOURCE_FILES_INC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "src/*.h" "src/*.hpp") 43 | file(GLOB PROJECT_SOURCE_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "src/*.cpp") 44 | 45 | if (PROJECT_SOURCE_FILES_SRC) 46 | set(LIBRARY_NAME ${PROJECT_NAME}) 47 | endif() 48 | 49 | ################################### 50 | ## catkin specific configuration ## 51 | ################################### 52 | # The catkin_package macro generates cmake config files for your package 53 | # Declare things to be passed to dependent projects 54 | # INCLUDE_DIRS: uncomment this if you package contains header files 55 | # LIBRARIES: libraries you create in this project that dependent projects also need 56 | # CATKIN_DEPENDS: catkin_packages dependent projects also need 57 | # DEPENDS: system dependencies of this project that dependent projects also need 58 | catkin_package( 59 | INCLUDE_DIRS include ${mrt_EXPORT_INCLUDE_DIRS} 60 | LIBRARIES ${LIBRARY_NAME} ${mrt_EXPORT_LIBRARIES} 61 | CATKIN_DEPENDS ${catkin_EXPORT_DEPENDS} 62 | ) 63 | 64 | ########### 65 | ## Build ## 66 | ########### 67 | # Add include and library directories 68 | include_directories( 69 | include/${PROJECT_NAME} 70 | ${mrt_INCLUDE_DIRS} 71 | ${catkin_INCLUDE_DIRS} 72 | ) 73 | 74 | link_directories( 75 | ${mrt_LIBRARY_DIRS} 76 | ) 77 | 78 | # Declare a cpp library 79 | mrt_add_library(${PROJECT_NAME} 80 | INCLUDES ${PROJECT_HEADER_FILES_INC} ${PROJECT_SOURCE_FILES_INC} 81 | SOURCES ${PROJECT_SOURCE_FILES_SRC} 82 | ) 83 | 84 | ############# 85 | ## Install ## 86 | ############# 87 | # Install all targets, headers by default and scripts and other files if specified (folders or files) 88 | mrt_install(PROGRAMS scripts FILES res data) 89 | 90 | ############# 91 | ## Testing ## 92 | ############# 93 | # Add test targets for cpp and python tests 94 | if (CATKIN_ENABLE_TESTING) 95 | mrt_add_tests(test) 96 | mrt_add_nosetests(test) 97 | endif() 98 | -------------------------------------------------------------------------------- /matches_msg_types/README.md: -------------------------------------------------------------------------------- 1 | # Matches Msg Types 2 | 3 | Non ros types for matches msgs for use in libraries 4 | 5 | ## Credits 6 | 7 | Johannes Gräter -------------------------------------------------------------------------------- /matches_msg_types/include/matches_msg_types/feature_point.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | namespace matches_msg_types { 4 | struct FeaturePoint { 5 | FeaturePoint() { 6 | ; 7 | } 8 | FeaturePoint(Eigen::Vector2d p) : u(p[0]), v(p[1]), d(-1) { 9 | ; 10 | } 11 | 12 | FeaturePoint(Eigen::Vector3d p) : u(p[0]), v(p[1]), d(p[2]) { 13 | ; 14 | } 15 | 16 | FeaturePoint(float u, float v) : u(u), v(v), d(-1) { 17 | ; 18 | } 19 | 20 | FeaturePoint(float u, float v, float d) : u(u), v(v), d(d) { 21 | ; 22 | } 23 | 24 | float u; 25 | float v; 26 | float d; 27 | 28 | Eigen::Vector2d toEigen2d() const { 29 | return Eigen::Vector2d(u, v); 30 | } 31 | 32 | Eigen::Vector3d toEigen3d() const { 33 | return Eigen::Vector3d(u, v, d); 34 | } 35 | 36 | }; 37 | } 38 | -------------------------------------------------------------------------------- /matches_msg_types/include/matches_msg_types/feature_point_depth.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "feature_point.hpp" 4 | namespace matches_msg_types { 5 | struct FeaturePointDepth : public FeaturePoint { 6 | FeaturePointDepth() { 7 | ; 8 | } 9 | 10 | FeaturePointDepth(Eigen::Vector3d p) 11 | : FeaturePoint(p[0], p[1]), d(p[2]) { 12 | ; 13 | } 14 | 15 | FeaturePointDepth(float u, float v, float d) 16 | : FeaturePoint(u, v), d(d) { 17 | ; 18 | } 19 | 20 | float d; 21 | }; 22 | } 23 | -------------------------------------------------------------------------------- /matches_msg_types/include/matches_msg_types/internal/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/matches_msg_types/include/matches_msg_types/internal/.gitignore -------------------------------------------------------------------------------- /matches_msg_types/include/matches_msg_types/tracklet.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "feature_point.hpp" 4 | namespace matches_msg_types { 5 | struct Tracklet { 6 | std::vector feature_points; 7 | 8 | unsigned long id; 9 | unsigned long age; 10 | bool is_outlier{false}; 11 | int label{-2}; 12 | }; 13 | } 14 | -------------------------------------------------------------------------------- /matches_msg_types/include/matches_msg_types/tracklets.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "tracklet.hpp" 5 | 6 | namespace matches_msg_types { 7 | 8 | using TimestampNSec = uint64_t; ///< Timestamp in unix system time 9 | 10 | struct Tracklets { 11 | std::vector stamps; 12 | std::vector tracks; 13 | }; 14 | } 15 | -------------------------------------------------------------------------------- /matches_msg_types/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | matches_msg_types 4 | 0.0.0 5 | ToDo: Edit package description 6 | 7 | contact-the-maintainer 8 | Johannes Graeter 9 | Johannes Graeter 10 | https://gitlab.mrt.uni-karlsruhe.de/MRT/matches_msg_types.git 11 | 12 | catkin 13 | mrt_cmake_modules 14 | gtest 15 | 16 | eigen 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /matches_msg_types/src/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/matches_msg_types/src/.gitignore -------------------------------------------------------------------------------- /matches_msg_types/test/matches_msg_types.cpp: -------------------------------------------------------------------------------- 1 | //google test docs 2 | //wiki page: https://code.google.com/p/googletest/w/list 3 | //primer: https://code.google.com/p/googletest/wiki/V1_7_Primer 4 | //FAQ: https://code.google.com/p/googletest/wiki/FAQ 5 | //advanced guide: https://code.google.com/p/googletest/wiki/V1_7_AdvancedGuide 6 | //samples: https://code.google.com/p/googletest/wiki/V1_7_Samples 7 | // 8 | //List of some basic tests fuctions: 9 | //Fatal assertion Nonfatal assertion Verifies / Description 10 | //------------------------------------------------------------------------------------------------------------------------------------------------------- 11 | //ASSERT_EQ(expected, actual); EXPECT_EQ(expected, actual); expected == actual 12 | //ASSERT_NE(val1, val2); EXPECT_NE(val1, val2); val1 != val2 13 | //ASSERT_LT(val1, val2); EXPECT_LT(val1, val2); val1 < val2 14 | //ASSERT_LE(val1, val2); EXPECT_LE(val1, val2); val1 <= val2 15 | //ASSERT_GT(val1, val2); EXPECT_GT(val1, val2); val1 > val2 16 | //ASSERT_GE(val1, val2); EXPECT_GE(val1, val2); val1 >= val2 17 | // 18 | //ASSERT_FLOAT_EQ(expected, actual); EXPECT_FLOAT_EQ(expected, actual); the two float values are almost equal (4 ULPs) 19 | //ASSERT_DOUBLE_EQ(expected, actual); EXPECT_DOUBLE_EQ(expected, actual); the two double values are almost equal (4 ULPs) 20 | //ASSERT_NEAR(val1, val2, abs_error); EXPECT_NEAR(val1, val2, abs_error); the difference between val1 and val2 doesn't exceed the given absolute error 21 | // 22 | //Note: more information about ULPs can be found here: http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm 23 | // 24 | //Example of two unit test: 25 | //TEST(Math, Add) { 26 | // ASSERT_EQ(10, 5+ 5); 27 | //} 28 | // 29 | //TEST(Math, Float) { 30 | // ASSERT_FLOAT_EQ((10.0f + 2.0f) * 3.0f, 10.0f * 3.0f + 2.0f * 3.0f) 31 | //} 32 | //======================================================================================================================================================= 33 | #include "gtest/gtest.h" 34 | 35 | //A google test function (uncomment the next function, add code and 36 | //change the names TestGroupName and TestName) 37 | //TEST(TestGroupName, TestName) { 38 | //TODO: Add your test code here 39 | //} 40 | 41 | -------------------------------------------------------------------------------- /pointcloud plotting/accumulate_lidar_pcl_from_trajectory_estimate.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import os 3 | 4 | import numpy as np 5 | import pykitti 6 | 7 | import vtk_pointcloud as vtk_pcl 8 | 9 | import argparse 10 | 11 | 12 | def main(sequence, results_dir, kitti_data_dir, start_end_increment=None): 13 | poses_name = results_dir+"/{}.txt".format(sequence) 14 | 15 | if start_end_increment not is None: # Get range of data. 16 | r_start = start_end_increment[0] 17 | r_end = start_end_increment[1] 18 | r_incr = start_end_increment[2] 19 | plot_range = range(r_start, r_end, r_incr) 20 | data = pykitti.odometry(kitti_data_dir, sequence, frames=plot_range) 21 | else: # Get all. 22 | data = pykitti.odometry(kitti_data_dir, sequence) 23 | 24 | # Find all the Velodyne files 25 | velo_path = os.path.join( 26 | kitti_data_dir, 'sequences', sequence, 'velodyne_points', 'data', '*.bin') 27 | velo_files = sorted(glob.glob(velo_path)) 28 | 29 | image_path = os.path.join( 30 | kitti_data_dir, 'sequences', sequence, 'image_2', 'data', '*.png') 31 | image_files = sorted(glob.glob(image_path)) 32 | 33 | # Create the geometry of a point (the coordinate) 34 | pcl_colored = vtk_pcl.VtkPointCloud("rgb") 35 | pcl = vtk_pcl.VtkPointCloud("y") 36 | 37 | # load estimated poses 38 | estimate = np.loadtxt(poses_name) 39 | for i in plot_range: 40 | pose = np.concatenate((estimate[i, :].reshape(3, 4), np.array([[0, 0, 0, 1]]))) 41 | 42 | scan = next(pykitti.utils.get_velo_scans([velo_files[i]])) 43 | img = next(pykitti.utils.get_images([image_files[i]], "cv2")) 44 | for poly_data_colored in scan: 45 | reflectance = poly_data_colored[3] 46 | if reflectance > 0.1: 47 | poly_data_colored[3] = 1 48 | p_cam0 = data.calib.T_cam0_velo.dot(poly_data_colored) 49 | p_world = pose.dot(p_cam0) 50 | 51 | pcl.add_point(p_world) 52 | 53 | # Get color data and add to visualizer. 54 | uv1 = data.calib.P_rect_20.dot(p_cam0) 55 | uv1 /= uv1[2] 56 | uv1 = np.floor(uv1).astype(int) 57 | 58 | if p_cam0[2] > 0.1 and 0 < uv1[0] < img.shape[1] - 1 and 0 < uv1[1] < img.shape[0] - 1: 59 | color = img[uv1[1], uv1[0]] 60 | # Add to visualizer. 61 | pcl_colored.add_point(p_world, color) 62 | 63 | print("processed frame number {}".format(i)) 64 | 65 | pcl.set_height(-2, 10, use_median=True) 66 | # write new PLY 67 | pcl_colored.write("/tmp/pcl_colored_{}_range_{}_{}_{}.ply".format(sequence, r_start, r_end, r_incr)) 68 | print("wrote pcl_colored.ply to tmp") 69 | 70 | # write new PLY 71 | # pcl.z_max = pcl.z_min + 20 72 | pcl.write("/tmp/pcl_{}_range_{}_{}_{}.ply".format(sequence, r_start, r_end, r_incr)) 73 | print("wrote pcl.ply to tmp") 74 | 75 | vtk_pcl.render(pcl) 76 | 77 | 78 | if __name__ == "__main__": 79 | parser = argparse.ArgumentParser() 80 | parser.add_argument("--sequence", type=str, help="Sequence number.") 81 | parser.add_argument("--results", type=str, help="Directory where textfiles of results are stored.") 82 | parser.add_argument("--data", type=str, help="Path to kitti data (without parent folder of all sequences).") 83 | 84 | args = parser.parse_args() 85 | main(args.sequence, args.results, args.data) 86 | -------------------------------------------------------------------------------- /pointcloud plotting/vtk_viewer.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import vtk 3 | 4 | 5 | def main(filename): 6 | # Read and display 7 | reader = vtk.vtkPLYReader() 8 | reader.SetFileName(filename) 9 | reader.Update() 10 | 11 | mapper = vtk.vtkPolyDataMapper() 12 | mapper.SetInputConnection(reader.GetOutputPort()) 13 | 14 | actor = vtk.vtkActor() 15 | actor.SetMapper(mapper) 16 | 17 | renderer = vtk.vtkRenderer() 18 | render_window = vtk.vtkRenderWindow() 19 | render_window.AddRenderer(renderer) 20 | render_window_interactor = vtk.vtkRenderWindowInteractor() 21 | render_window_interactor.SetRenderWindow(render_window) 22 | 23 | renderer.AddActor(actor) 24 | renderer.SetBackground(0.1804, 0.5451, 0.3412) 25 | renderer.SetBackground(0.1804, 0.5451, 0.3412) 26 | 27 | render_window.Render() 28 | render_window_interactor.Start() 29 | 30 | if __name__ == "__main__": 31 | parser = argparse.ArgumentParser() 32 | parser.add_argument( 33 | "--file", type=str, help="path to file") 34 | 35 | args = parser.parse_args() 36 | main(args.file) 37 | -------------------------------------------------------------------------------- /robust_optimization/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | setup.py 3 | *.pyc 4 | CMakeLists.txt.user 5 | 6 | -------------------------------------------------------------------------------- /robust_optimization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MRT_PKG_VERSION 2.2) 2 | # Modify only if you know what you are doing! 3 | cmake_minimum_required(VERSION 2.8.12) 4 | project(robust_optimization) 5 | 6 | ################### 7 | ## find packages ## 8 | ################### 9 | find_package(mrt_cmake_modules REQUIRED) 10 | include(UseMrtStdCompilerFlags) 11 | include(UseMrtAutoTarget) 12 | 13 | include(GatherDeps) 14 | # Remove libs which cannot be found automatically 15 | #list(REMOVE_ITEM DEPENDEND_PACKAGES ...) 16 | find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES}) 17 | 18 | # Manually resolve removed dependend packages 19 | #find_package(...) 20 | 21 | # Mark files or folders for display in IDEs 22 | mrt_add_to_ide(README.md .gitlab-ci.yml) 23 | 24 | ######################## 25 | ## add python modules ## 26 | ######################## 27 | # This adds a python module if located under src/{PROJECT_NAME) 28 | mrt_python_module_setup() 29 | 30 | file(GLOB PROJECT_PYTHON_SOURCE_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "python_api/*.cpp") 31 | if (PROJECT_PYTHON_SOURCE_FILES_SRC) 32 | # Add a cpp-python api library. Make sure there are no name collisions with python modules in this project 33 | mrt_add_python_api( ${PROJECT_NAME} 34 | FILES ${PROJECT_PYTHON_SOURCE_FILES_SRC} 35 | ) 36 | endif() 37 | 38 | ############################ 39 | ## read source code files ## 40 | ############################ 41 | file(GLOB_RECURSE PROJECT_HEADER_FILES_INC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "include/*.h" "include/*.hpp") 42 | file(GLOB PROJECT_SOURCE_FILES_INC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "src/*.h" "src/*.hpp") 43 | file(GLOB PROJECT_SOURCE_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "src/*.cpp") 44 | 45 | if (PROJECT_SOURCE_FILES_SRC) 46 | set(LIBRARY_NAME ${PROJECT_NAME}) 47 | endif() 48 | 49 | ################################### 50 | ## catkin specific configuration ## 51 | ################################### 52 | # The catkin_package macro generates cmake config files for your package 53 | # Declare things to be passed to dependent projects 54 | # INCLUDE_DIRS: uncomment this if you package contains header files 55 | # LIBRARIES: libraries you create in this project that dependent projects also need 56 | # CATKIN_DEPENDS: catkin_packages dependent projects also need 57 | # DEPENDS: system dependencies of this project that dependent projects also need 58 | catkin_package( 59 | INCLUDE_DIRS include ${mrt_EXPORT_INCLUDE_DIRS} 60 | LIBRARIES ${LIBRARY_NAME} ${mrt_EXPORT_LIBRARIES} 61 | CATKIN_DEPENDS ${catkin_EXPORT_DEPENDS} 62 | ) 63 | 64 | ########### 65 | ## Build ## 66 | ########### 67 | # Add include and library directories 68 | include_directories( 69 | include/${PROJECT_NAME} 70 | ${mrt_INCLUDE_DIRS} 71 | ${catkin_INCLUDE_DIRS} 72 | ) 73 | 74 | link_directories( 75 | ${mrt_LIBRARY_DIRS} 76 | ) 77 | 78 | # Declare a cpp library 79 | mrt_add_library(${PROJECT_NAME} 80 | INCLUDES ${PROJECT_HEADER_FILES_INC} ${PROJECT_SOURCE_FILES_INC} 81 | SOURCES ${PROJECT_SOURCE_FILES_SRC} 82 | ) 83 | 84 | ############# 85 | ## Install ## 86 | ############# 87 | # Install all targets, headers by default and scripts and other files if specified (folders or files) 88 | mrt_install(PROGRAMS scripts FILES res data) 89 | 90 | ############# 91 | ## Testing ## 92 | ############# 93 | # Add test targets for cpp and python tests 94 | if (CATKIN_ENABLE_TESTING) 95 | mrt_add_tests(test) 96 | mrt_add_nosetests(test) 97 | endif() 98 | -------------------------------------------------------------------------------- /robust_optimization/README.md: -------------------------------------------------------------------------------- 1 | # ROBUST OPTIMIZATION 2 | Wrapper library around ceres to implement trimmed least squares algorithm. 3 | 4 | ## Usage 5 | * Find a working example in ./test/robust_optimization.cpp 6 | * Given: 7 | * ceres problem (name: problem) as std::unique_ptr 8 | * residual block ids (name: residual_block_ids) as robust_optimization::ResidualIds 9 | 10 | ```cpp 11 | // Input for trimmed least squares. 12 | int number_iterations = 3; // Set number of iterations of trimmed least squares. 13 | int number_steps = 2; 14 | 15 | std::vector number_iterations{}; 16 | for (int i = 0; i < number_iterations; i++) { 17 | number_iterations.push_back(number_steps); 18 | } 19 | std::vector> input; 20 | input.push_back( 21 | std::make_pair(residual_block_ids, 22 | robust_optimization::TrimmerSpecification(robust_optimization::TrimmerType::Quantile, 23 | outlier_rejection_options_.depth_quantile))); 24 | 25 | robust_optimization::Options opt = robust_optimization::getStandardSolverOptions(solver_time_sec); 26 | opt.max_solver_time_refinement_in_seconds = solver_time_sec; 27 | opt.minimum_number_residual_groups = 30; 28 | opt.trust_region_relaxation_factor = -10.; // Reset trust region at each iteration. 29 | opt.num_threads = 3; 30 | auto final_summary = robust_optimization::solveTrimmed(number_iterations, input, *problem, opt); 31 | std::cout << final_summary.FullReport() << std::endl; 32 | ``` 33 | -------------------------------------------------------------------------------- /robust_optimization/include/robust_optimization/internal/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/robust_optimization/include/robust_optimization/internal/.gitignore -------------------------------------------------------------------------------- /robust_optimization/include/robust_optimization/internal/apply_trimmer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | #include 11 | #include 12 | 13 | #include "definitions.hpp" 14 | 15 | #include "trimmer_fix.hpp" 16 | #include "trimmer_quantile.hpp" 17 | 18 | namespace robust_optimization { 19 | 20 | /** 21 | * @brief This a factory that constructs a trimmer and applies it. 22 | * if you implement a new trimmer, register it as a type and 23 | * in apply_trimmer. 24 | * @param data; data to be trimmed, 25 | * templated since we do not care about ids. 26 | * @param type; filter type to be applied. 27 | * @return outliers; return Ids of outliers. 28 | */ 29 | template 30 | std::vector getOutliers(const std::map& data, TrimmerType type, Args... filter_args) { 31 | switch (type) { 32 | case TrimmerType::Fix: { 33 | TrimmerFix t(filter_args...); 34 | return t.getOutliers(data); 35 | } 36 | case TrimmerType::Quantile: { 37 | TrimmerQuantile t(filter_args...); 38 | return t.getOutliers(data); 39 | } 40 | default: 41 | std::runtime_error("In apply_trimmer: TrimmerType not defined"); 42 | } 43 | 44 | return std::vector{}; 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /robust_optimization/include/robust_optimization/internal/definitions.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | namespace robust_optimization { 12 | enum class TrimmerType { Fix, Quantile }; 13 | } 14 | -------------------------------------------------------------------------------- /robust_optimization/include/robust_optimization/internal/trimmer_fix.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | namespace robust_optimization { 15 | /** 16 | * @class TrimmerFix 17 | * @par 18 | * 19 | * Most simple concrete trimmer. 20 | * Uses fix residual threshold for outlier rejection. 21 | */ 22 | class TrimmerFix { 23 | public: // Public methods. 24 | // Default constructor. 25 | explicit TrimmerFix(double outlier_threshold) : outlier_thres_(outlier_threshold) { 26 | ; 27 | } 28 | 29 | // Default move. 30 | TrimmerFix(TrimmerFix&& other) = default; 31 | TrimmerFix& operator=(TrimmerFix&& other) = default; 32 | 33 | // Default copy. 34 | TrimmerFix(const TrimmerFix& other) = default; 35 | TrimmerFix& operator=(const TrimmerFix& other) = default; 36 | 37 | template 38 | std::vector getOutliers(const std::map& residuals_input) { 39 | std::vector outliers; 40 | for (const auto& el : residuals_input) { 41 | if (el.second > outlier_thres_) { 42 | outliers.push_back(el.first); 43 | } 44 | } 45 | return outliers; 46 | } 47 | 48 | private: 49 | double outlier_thres_; ///< Fix threshold. Residuals > thres will be rejected. 50 | }; 51 | } // end of namespace 52 | -------------------------------------------------------------------------------- /robust_optimization/include/robust_optimization/internal/trimmer_quantile.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018. All rights reserved. 2 | // Institute of Measurement and Control Systems 3 | // Karlsruhe Institute of Technology, Germany 4 | // 5 | // authors: 6 | // Johannes Graeter (johannes.graeter@kit.edu) 7 | // and others 8 | 9 | #pragma once 10 | #include 11 | #include 12 | #include 13 | 14 | namespace robust_optimization { 15 | /** 16 | * @class TrimmerQuantile 17 | * @par 18 | * 19 | * Trimms data with quantiles. 20 | * if quantile=0.9, the highest 10 % of redisuals are outliers 21 | */ 22 | class TrimmerQuantile { 23 | public: // Public classes/enums/types etc... 24 | public: // Attributes. 25 | public: // Public methods. 26 | // Default constructor. 27 | explicit TrimmerQuantile(double quantile) : quantile_(quantile) { 28 | ; 29 | } 30 | 31 | // Default move. 32 | TrimmerQuantile(TrimmerQuantile&& other) = default; 33 | TrimmerQuantile& operator=(TrimmerQuantile&& other) = default; 34 | 35 | // Default copy. 36 | TrimmerQuantile(const TrimmerQuantile& other) = default; 37 | TrimmerQuantile& operator=(const TrimmerQuantile& other) = default; 38 | 39 | template 40 | std::vector getOutliers(const std::map& residuals_input) { 41 | // Copy map to vector for sorting. 42 | std::vector> input_vec; 43 | input_vec.resize(residuals_input.size()); 44 | std::transform( 45 | residuals_input.cbegin(), residuals_input.cend(), input_vec.begin(), [](const auto& a) { return a; }); 46 | 47 | // Calc element number that corresponds to quantile. 48 | int num = static_cast(static_cast(input_vec.size()) * quantile_); 49 | 50 | // From http://en.cppreference.com/w/cpp/algorithm/nth_element 51 | // All of the elements before this new nth element are less than or equal to the elements after the new nth 52 | // element. 53 | std::nth_element(input_vec.begin(), input_vec.begin() + num, input_vec.end(), [](const auto& a, const auto& b) { 54 | return a.second < b.second; 55 | }); 56 | 57 | std::vector outliers; 58 | for (auto it = input_vec.cbegin() + num; it != input_vec.cend(); it++) { 59 | outliers.push_back(it->first); 60 | } 61 | return outliers; 62 | } 63 | 64 | private: 65 | double quantile_; ///< Quantile for rejection, between 0. and 1.0, 0.9 means 10% largest residuals are rejected. 66 | }; 67 | } 68 | -------------------------------------------------------------------------------- /robust_optimization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robust_optimization 4 | 0.0.0 5 | ToDo: Edit package description 6 | 7 | contact-the-maintainer 8 | Johannes Graeter 9 | Johannes Graeter 10 | https://gitlab.mrt.uni-karlsruhe.de/MRT/robust_optimization 11 | 12 | catkin 13 | mrt_cmake_modules 14 | gtest 15 | mrt_ceres 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /robust_optimization/src/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/robust_optimization/src/.gitignore -------------------------------------------------------------------------------- /src/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/src/.gitkeep -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/.gitignore: -------------------------------------------------------------------------------- 1 | *~ -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MRT_PKG_VERSION 2.2) 2 | # Modify only if you know what you are doing! 3 | cmake_minimum_required(VERSION 2.8.12) 4 | project(util_nodes_tf2_ros_tool) 5 | 6 | ################### 7 | ## find packages ## 8 | ################### 9 | find_package(mrt_cmake_modules REQUIRED) 10 | include(UseMrtStdCompilerFlags) 11 | include(UseMrtAutoTarget) 12 | 13 | include(GatherDeps) 14 | # Remove libs which cannot be found automatically 15 | #list(REMOVE_ITEM DEPENDEND_PACKAGES ...) 16 | find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES}) 17 | 18 | # Manually resolve removed dependend packages 19 | #find_package(...) 20 | 21 | # Mark files or folders for display in IDEs 22 | mrt_add_to_ide(README.md .gitlab-ci.yml) 23 | 24 | ######################## 25 | ## add python modules ## 26 | ######################## 27 | # This adds a python module if located under src/{PROJECT_NAME) 28 | mrt_python_module_setup() 29 | 30 | ################################################ 31 | ## Declare ROS messages, services and actions ## 32 | ################################################ 33 | 34 | # Add message, service and action files 35 | glob_ros_files(add_message_files msg) 36 | glob_ros_files(add_service_files srv) 37 | glob_ros_files(add_action_files action) 38 | 39 | # Generate added messages and services with any dependencies listed here 40 | if (ROS_GENERATE_MESSAGES) 41 | generate_messages( 42 | DEPENDENCIES 43 | #add dependencies here 44 | #std_msgs 45 | ) 46 | endif() 47 | 48 | # Generate dynamic reconfigure options 49 | file(GLOB PARAMS_FILES RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "cfg/*.params" "cfg/*.cfg" "cfg/*.mrtcfg" "cfg/*.rosif") 50 | if (PARAMS_FILES) 51 | generate_ros_parameter_files(${PARAMS_FILES}) 52 | generate_ros_interface_files(${PARAMS_FILES}) 53 | endif () 54 | 55 | ################################### 56 | ## catkin specific configuration ## 57 | ################################### 58 | catkin_package( 59 | ) 60 | 61 | ########### 62 | ## Build ## 63 | ########### 64 | # Add include and library directories 65 | include_directories( 66 | ${mrt_INCLUDE_DIRS} 67 | ${catkin_INCLUDE_DIRS} 68 | ) 69 | 70 | link_directories( 71 | ${mrt_LIBRARY_DIRS} 72 | ) 73 | 74 | glob_folders(SRC_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/src") 75 | if (SRC_DIRECTORIES) 76 | # Found subfolders, add executable for each subfolder 77 | foreach(SRC_DIR ${SRC_DIRECTORIES}) 78 | mrt_add_node_and_nodelet(${SRC_DIR} FOLDER "src/${SRC_DIR}") 79 | endforeach() 80 | else() 81 | # No subfolder found, add executable and python modules for src folder 82 | mrt_add_node_and_nodelet(${PROJECT_NAME} FOLDER "src") 83 | endif() 84 | 85 | ############# 86 | ## Install ## 87 | ############# 88 | # Install all targets, headers by default and scripts and other files if specified (folders or files) 89 | mrt_install(PROGRAMS scripts FILES launch rviz maps res data nodelet_plugins.xml plugin_description.xml) 90 | 91 | ############# 92 | ## Testing ## 93 | ############# 94 | # Add test targets for cpp and python tests 95 | if (CATKIN_ENABLE_TESTING) 96 | # Include header in src folder via src//.h 97 | include_directories("src") 98 | mrt_add_ros_tests(test) 99 | mrt_add_nosetests(test) 100 | endif() 101 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/README.md: -------------------------------------------------------------------------------- 1 | # Project Name 2 | 3 | TODO: Write a project description 4 | 5 | ## Installation 6 | 7 | TODO: Describe the installation process 8 | 9 | ## Usage 10 | 11 | TODO: Write usage instructions 12 | 13 | ## Contributing 14 | 15 | 1. Fork it! 16 | 2. Create your feature branch: `git checkout -b my-new-feature` 17 | 3. Commit your changes: `git commit -am 'Add some feature'` 18 | 4. Push to the branch: `git push origin my-new-feature` 19 | 5. Submit a pull request :D 20 | 21 | ## History 22 | 23 | TODO: Write history 24 | 25 | ## Credits 26 | 27 | TODO: Write credits 28 | 29 | ## License 30 | 31 | This program is free software: you can redistribute it and/or modify 32 | it under the terms of the GNU General Public License as published by 33 | the Free Software Foundation, either version 3 of the License, or 34 | (at your option) any later version. 35 | 36 | This program is distributed in the hope that it will be useful, 37 | but WITHOUT ANY WARRANTY; without even the implied warranty of 38 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 39 | GNU General Public License for more details. 40 | 41 | You should have received a copy of the GNU General Public License 42 | along with this program. If not, see . -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/cfg/StaticTransformAlias.rosif: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Add your desired parameters here. All required headers will be generated from this. 4 | !!!IMPORTANT this file must be executable !!! 5 | 6 | Use one of these commands to add parameters to your parameter struct. 7 | 8 | def add(self, name, paramtype, description, level=0, edit_method='""', default=None, min=None, max=None, configurable=False, global_scope=False, constant=False): 9 | Adds parameters to your parameter struct. 10 | 11 | - If no default value is given, you need to specify one in your launch file 12 | - Global parameters, vectors, maps and constant params can not be configurable 13 | - Global parameters, vectors and maps can not have a default, min or max value 14 | 15 | :param self: 16 | :param name: The Name of you new parameter 17 | :param paramtype: The C++ type of this parameter. Can be any of ['std::string', 'int', 'bool', 'float', 'double'] or std::vector<...> or std::map 18 | :param description: Choose an informative documentation string for this parameter. 19 | :param level: (optional) Passed to dynamic_reconfigure 20 | :param edit_method: (optional) Passed to dynamic_reconfigure 21 | :param default: (optional) default value 22 | :param min: (optional) 23 | :param max: (optional) 24 | :param configurable: (optional) Should this parameter be dynamic configurable 25 | :param global_scope: (optional) If true, parameter is searched in global ('/') namespace instead of private ('~') ns 26 | :param constant: (optional) If this is true, the parameter will not be fetched from param server, but the default value is kept. 27 | :return: None 28 | 29 | def add_enum(self, name, description, entry_strings, default=None): 30 | Adds an enum to dynamic reconfigure 31 | :param name: Name of enum parameter 32 | :param description: Informative documentation string 33 | :param entry_strings: Enum entries, must be strings! (will be numbered with increasing value) 34 | :param default: Default value 35 | :return: 36 | 37 | """ 38 | from rosinterface_handler.interface_generator_catkin import * 39 | 40 | gen = InterfaceGenerator() 41 | 42 | # ROS-specific 43 | gen.add("msg_queue_size", paramtype='int', description="Queusize for publisher", default=5, min=1) 44 | gen.add_verbosity_param(configurable=True) 45 | 46 | # Diagnostics 47 | gen.add("diag_pub_msg_name", paramtype='std::string', description="Topicname for diagnostic publisher", default="out_topic_diagnosed") 48 | gen.add("diagnostic_updater_name", paramtype='std::string', description="Name of diagnostic updater.", default="StaticTransformAliasUpdater") 49 | gen.add("diagnostic_updater_hardware_id", paramtype='std::string', description="Identifier for hardware.", default="StaticTransformAlias") 50 | gen.add("diagnostic_updater_rate", paramtype='double', description="Expected updater frequency", default=1) 51 | gen.add("diagnostic_updater_rate_tolerance", paramtype='double', description="Tolerance with which bounds must be satisfied.", default=1) 52 | 53 | # Your Params here 54 | gen.add("from_source_frame_id", paramtype='std::string', description="tf2 frame id of the source frame from which to read", default="from/source", configurable=True) 55 | gen.add("to_source_frame_id", paramtype='std::string', description="tf2 frame id of the source frame to which we map", default="to/source", configurable=True) 56 | 57 | gen.add("from_target_frame_id", paramtype='std::string', description="tf2 frame id of the target frame from which to read", default="from/target", configurable=True) 58 | gen.add("to_target_frame_id", paramtype='std::string', description="tf2 frame id of the target frame to which we map", default="to/target", configurable=True) 59 | 60 | gen.add("timeout", paramtype='double', description="time in secondsd before failing", default="10.", min=0.5, max=60, configurable=True) 61 | 62 | # DO NOT TOUCH THIS LINE 63 | #Syntax : Package, Node, Config Name(The final name will be StaticTransformAliasConfig) 64 | exit(gen.generate("util_nodes_tf2_ros_tool", "static_transform_alias", "StaticTransformAlias")) 65 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/launch/params/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/util_nodes_tf2_ros_tool/launch/params/.gitignore -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/launch/params/static_transform_alias_parameters.yaml: -------------------------------------------------------------------------------- 1 | # Use this file to specify parameters, that do not have default values. 2 | # YAML Files can also be helpfull if you want to save parameters for different use-cases. 3 | # In any way: Default values should go into the .mrtcfg file! 4 | # 5 | # Use 'key: value' pairs, e.g. 6 | # string: 'foo' 7 | # integer: 1234 8 | # float: 1234.5 9 | # boolean: true 10 | # vector: [1.0, 2.0, 3.4] 11 | # map: {"a": "b", "c": "d"} 12 | 13 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/launch/static_transform_alias_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/launch/static_transform_alias_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/package.xml: -------------------------------------------------------------------------------- 1 | 2 | util_nodes_tf2_ros_tool 3 | 0.0.0 4 | ToDo: Edit package description 5 | 6 | GPLv3 7 | Johannes Graeter 8 | Johannes Graeter 9 | https://gitlab.mrt.uni-karlsruhe.de/MRT/util_nodes_tf2_ros_tool.git 10 | 11 | catkin 12 | mrt_cmake_modules 13 | rosinterface_handler 14 | message_generation 15 | message_runtime 16 | gtest 17 | rostest 18 | 19 | roscpp 20 | 21 | roslib 22 | nodelet 23 | 24 | 38 | 42 | 43 | dynamic_reconfigure 44 | tf2_ros 45 | tf 46 | diagnostic_updater 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/src/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/johannes-graeter/limo/7634c63543c53cea09b3efabf8d0e1a54dfe1a3a/util_nodes_tf2_ros_tool/src/.gitignore -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/src/static_transform_alias/static_transform_alias.cpp: -------------------------------------------------------------------------------- 1 | #include "static_transform_alias.hpp" 2 | #include 3 | #include 4 | 5 | namespace util_nodes_tf2_ros_tool { 6 | 7 | StaticTransformAlias::StaticTransformAlias(ros::NodeHandle node_handle, ros::NodeHandle private_node_handle) 8 | : reconfigSrv_{private_node_handle}, params_{private_node_handle}, tfListener_{tfBuffer_} { 9 | 10 | /** 11 | * Initialization 12 | */ 13 | rosinterface_handler::setLoggerLevel(private_node_handle); 14 | params_.fromParamServer(); 15 | setupDiagnostics(); 16 | 17 | /** 18 | * Set up dynamic reconfiguration 19 | */ 20 | reconfigSrv_.setCallback(boost::bind(&StaticTransformAlias::reconfigureRequest, this, _1, _2)); 21 | 22 | /** 23 | * Publishers & subscriber 24 | */ 25 | // A diagnosed pub can be used for message types with header. 26 | // This adds a diagnostics message for the frequency to this topic 27 | // diagnosed_pub_ = std::make_unique>( 28 | // private_node_handle.advertise(params_.diag_pub_msg_name, 29 | // params_.msg_queue_size), 30 | // updater_, 31 | // diagnostic_updater::FrequencyStatusParam(¶ms_.diagnostic_updater_rate, 32 | // ¶ms_.diagnostic_updater_rate, 33 | // params_.diagnostic_updater_rate_tolerance, 5), 34 | // diagnostic_updater::TimeStampStatusParam()); 35 | 36 | // dummyPub_ = private_node_handle.advertise(params_.publisher_msg_name, 37 | // params_.msg_queue_size); 38 | // // Instantiate subscriber last, to assure all objects are initialised when first message 39 | // is received. 40 | // dummySub_ = 41 | // private_node_handle.subscribe(params_.subscriber_msg_name, params_.msg_queue_size, 42 | // &StaticTransformAlias::subCallback, this, 43 | // ros::TransportHints().tcpNoDelay()); 44 | 45 | do_aliasing(); 46 | 47 | rosinterface_handler::showNodeInfo(); 48 | } 49 | 50 | void StaticTransformAlias::do_aliasing() { 51 | ROS_DEBUG_STREAM("looking up transform from " << params_.from_target_frame_id << " to " 52 | << params_.to_target_frame_id 53 | << " timeout=" 54 | << params_.timeout 55 | << " sec"); 56 | std::string frame_name_target, frame_name_source; 57 | tf::resolve(params_.from_target_frame_id, frame_name_target); 58 | tf::resolve(params_.from_source_frame_id, frame_name_source); 59 | geometry_msgs::TransformStamped transform = 60 | tfBuffer_.lookupTransform(frame_name_target, frame_name_source, ros::Time(0), ros::Duration(params_.timeout)); 61 | transform.header.frame_id = params_.to_target_frame_id; 62 | transform.child_frame_id = params_.to_source_frame_id; 63 | tfBroadcaster_.sendTransform(transform); 64 | } 65 | 66 | ///* 67 | // * Use const ConstPtr for your callbacks. 68 | // * The 'const' assures that you can not edit incoming messages. 69 | // * The Ptr type guarantees zero copy transportation within nodelets. 70 | // */ 71 | // void StaticTransformAlias::subCallback(const std_msgs::Header::ConstPtr& msg) { 72 | 73 | // // do your stuff here... 74 | // std_msgs::Header new_msg = *msg; 75 | // dummyPub_.publish(new_msg); 76 | // // diagnosed_pub_->publish(new_msg); 77 | 78 | // diagnosticStatus_.message = "Valid loop"; 79 | // diagnosticStatus_.level = diagnostic_msgs::DiagnosticStatus::OK; 80 | // // The updater will take care of publishing at a throttled rate 81 | // // When calling update, all updater callbacks (defined in setupDiagnostics) will be run 82 | // updater_.update(); 83 | //} 84 | 85 | /** 86 | * This callback is called whenever a change was made in the dynamic_reconfigure window 87 | */ 88 | void StaticTransformAlias::reconfigureRequest(StaticTransformAliasConfig& config, uint32_t level) { 89 | params_.fromConfig(config); 90 | 91 | do_aliasing(); 92 | } 93 | 94 | /* 95 | * Setup the Diagnostic Updater 96 | */ 97 | void StaticTransformAlias::setupDiagnostics() { 98 | // Give a unique hardware id 99 | diagnosticStatus_.hardware_id = params_.diagnostic_updater_hardware_id; 100 | diagnosticStatus_.message = "Starting..."; 101 | diagnosticStatus_.level = diagnostic_msgs::DiagnosticStatus::STALE; 102 | updater_.setHardwareID(params_.diagnostic_updater_hardware_id); 103 | 104 | // Add further callbacks (or unittests) that should be called regularly 105 | updater_.add("StaticTransformAlias Sensor Status", this, &StaticTransformAlias::checkSensorStatus); 106 | 107 | updater_.force_update(); 108 | } 109 | 110 | void StaticTransformAlias::checkSensorStatus(diagnostic_updater::DiagnosticStatusWrapper& status_wrapper) { 111 | status_wrapper.summary(diagnosticStatus_); 112 | } 113 | 114 | } // namespace util_nodes_tf2_ros_tool 115 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/src/static_transform_alias/static_transform_alias.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "util_nodes_tf2_ros_tool/StaticTransformAliasInterface.h" 11 | 12 | namespace util_nodes_tf2_ros_tool { 13 | 14 | class StaticTransformAlias { 15 | public: 16 | StaticTransformAlias(ros::NodeHandle, ros::NodeHandle); 17 | 18 | private: 19 | ros::Publisher dummyPub_; 20 | ros::Subscriber dummySub_; 21 | 22 | StaticTransformAliasInterface params_; 23 | 24 | dynamic_reconfigure::Server reconfigSrv_; // Dynamic reconfiguration service 25 | 26 | tf2_ros::Buffer tfBuffer_; 27 | tf2_ros::TransformListener tfListener_; 28 | tf2_ros::StaticTransformBroadcaster tfBroadcaster_; 29 | 30 | /// Diagnostics 31 | diagnostic_updater::Updater updater_; 32 | // std::unique_ptr> diagnosed_pub_; 33 | diagnostic_msgs::DiagnosticStatus diagnosticStatus_; 34 | 35 | void setupDiagnostics(); 36 | void checkSensorStatus(diagnostic_updater::DiagnosticStatusWrapper&); 37 | void diagnostic_msg(diagnostic_updater::DiagnosticStatusWrapper&); 38 | void diagnoseError(); 39 | 40 | void subCallback(const std_msgs::Header::ConstPtr& msg); 41 | void reconfigureRequest(StaticTransformAliasConfig&, uint32_t); 42 | 43 | // execute aliasing, wait for timeout seconds before failing 44 | void do_aliasing(); 45 | }; 46 | 47 | } // namespace util_nodes_tf2_ros_tool 48 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/src/static_transform_alias/static_transform_alias_node.cpp: -------------------------------------------------------------------------------- 1 | #include "static_transform_alias.hpp" 2 | 3 | int main(int argc, char* argv[]) { 4 | 5 | ros::init(argc, argv, "static_transform_alias_node"); 6 | 7 | util_nodes_tf2_ros_tool::StaticTransformAlias static_transform_alias(ros::NodeHandle(), ros::NodeHandle("~")); 8 | 9 | ros::spin(); 10 | return 0; 11 | } 12 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/src/static_transform_alias/static_transform_alias_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include "static_transform_alias.hpp" 2 | #include 3 | #include 4 | 5 | namespace util_nodes_tf2_ros_tool { 6 | 7 | class StaticTransformAliasNodelet : public nodelet::Nodelet { 8 | 9 | virtual void onInit(); 10 | boost::shared_ptr m_; 11 | }; 12 | 13 | void StaticTransformAliasNodelet::onInit() { 14 | m_.reset(new StaticTransformAlias(getNodeHandle(), getPrivateNodeHandle())); 15 | } 16 | 17 | } // namespace util_nodes_tf2_ros_tool 18 | 19 | PLUGINLIB_EXPORT_CLASS(util_nodes_tf2_ros_tool::StaticTransformAliasNodelet, nodelet::Nodelet); 20 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/test/test_util_nodes_tf2_ros_tool.cpp: -------------------------------------------------------------------------------- 1 | //google test docs 2 | //wiki page: https://code.google.com/p/googletest/w/list 3 | //primer: https://code.google.com/p/googletest/wiki/V1_7_Primer 4 | //FAQ: https://code.google.com/p/googletest/wiki/FAQ 5 | //advanced guide: https://code.google.com/p/googletest/wiki/V1_7_AdvancedGuide 6 | //samples: https://code.google.com/p/googletest/wiki/V1_7_Samples 7 | // 8 | //List of some basic tests fuctions: 9 | //Fatal assertion Nonfatal assertion Verifies / Description 10 | //------------------------------------------------------------------------------------------------------------------------------------------------------- 11 | //ASSERT_EQ(expected, actual); EXPECT_EQ(expected, actual); expected == actual 12 | //ASSERT_NE(val1, val2); EXPECT_NE(val1, val2); val1 != val2 13 | //ASSERT_LT(val1, val2); EXPECT_LT(val1, val2); val1 < val2 14 | //ASSERT_LE(val1, val2); EXPECT_LE(val1, val2); val1 <= val2 15 | //ASSERT_GT(val1, val2); EXPECT_GT(val1, val2); val1 > val2 16 | //ASSERT_GE(val1, val2); EXPECT_GE(val1, val2); val1 >= val2 17 | // 18 | //ASSERT_FLOAT_EQ(expected, actual); EXPECT_FLOAT_EQ(expected, actual); the two float values are almost equal (4 ULPs) 19 | //ASSERT_DOUBLE_EQ(expected, actual); EXPECT_DOUBLE_EQ(expected, actual); the two double values are almost equal (4 ULPs) 20 | //ASSERT_NEAR(val1, val2, abs_error); EXPECT_NEAR(val1, val2, abs_error); the difference between val1 and val2 doesn't exceed the given absolute error 21 | // 22 | //Note: more information about ULPs can be found here: http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm 23 | // 24 | //Example of two unit test: 25 | //TEST(Math, Add) { 26 | // ASSERT_EQ(10, 5+ 5); 27 | //} 28 | // 29 | //TEST(Math, Float) { 30 | // ASSERT_FLOAT_EQ((10.0f + 2.0f) * 3.0f, 10.0f * 3.0f + 2.0f * 3.0f) 31 | //} 32 | //======================================================================================================================================================= 33 | #include "gtest/gtest.h" 34 | #include 35 | 36 | //A google test function (uncomment the next function, add code and 37 | //change the names TestGroupName and TestName) 38 | //TEST(TestGroupName, TestName) { 39 | //TODO: Add your test code here. You can use ros::NodeHandle and all other ros functionality here. 40 | //} 41 | 42 | int main(int argc, char **argv) { 43 | ros::init(argc, argv, "unittest"); 44 | testing::InitGoogleTest(&argc, argv); 45 | return RUN_ALL_TESTS(); 46 | } 47 | -------------------------------------------------------------------------------- /util_nodes_tf2_ros_tool/test/test_util_nodes_tf2_ros_tool.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | --------------------------------------------------------------------------------