├── yaml └── configs │ ├── empty.yaml │ ├── disable_debug_topics.yaml │ ├── covariance_estimation │ └── covariance_estimation.yaml │ ├── filters │ ├── filters_2d.yaml │ ├── filters_3d.yaml │ ├── filters_3d_slam.yaml │ ├── filters_large_map_2d.yaml │ ├── filters_large_map_2d_slam.yaml │ └── filters_3d_tof.yaml │ ├── pose_recovery │ ├── recovery_3d.yaml │ ├── recovery_3d_large_map.yaml │ ├── recovery_slam_3d.yaml │ ├── recovery_slam_2d.yaml │ └── recovery_2d.yaml │ └── pose_tracking │ ├── cluttered_environments_3d.yaml │ ├── unstable_ground_3d.yaml │ ├── cluttered_environments_dynamic_3d.yaml │ ├── unstable_ground_3d_tof.yaml │ ├── static_2d.yaml │ ├── static_fast_2d.yaml │ ├── unstable_ground_2d.yaml │ ├── cluttered_environments_2d.yaml │ ├── cluttered_environments_fast_2d.yaml │ ├── static_without_odometry_2d.yaml │ ├── static_3d.yaml │ ├── cluttered_environments_dynamic_2d.yaml │ ├── cluttered_environments_dynamic_fast_2d.yaml │ ├── cluttered_environments_dynamic_large_map_slam_2d.yaml │ └── static_3d_tof.yaml ├── srv ├── StopProcessingSensorData.srv ├── StartProcessingSensorData.srv └── ReloadLocalizationConfiguration.srv ├── tools ├── binvox ├── meshlab_filters.mlx ├── create_eclipse_projects.bash └── convert_mesh_to_pcd.bash ├── docs ├── overview.pdf ├── overview.png ├── system-modules.png ├── system-modules.pptx ├── perception-overview.pdf ├── perception-overview.png ├── cad_to_octree.md ├── octomap_commands.md ├── mesh_conversions.md └── mesh_tessellation.md ├── rosdoc.yaml ├── .gitignore ├── msg ├── LocalizationDiagnostics.msg ├── LocalizationTimes.msg ├── LocalizationDetailed.msg └── LocalizationConfiguration.msg ├── src ├── transformation_validators │ └── transformation_validator.cpp ├── cloud_filters │ ├── scale.cpp │ ├── crop_box.cpp │ ├── voxel_grid.cpp │ ├── cloud_filter.cpp │ ├── pass_through.cpp │ ├── random_sample.cpp │ ├── region_growing.cpp │ ├── hsv_segmentation.cpp │ ├── plane_segmentation.cpp │ ├── covariance_sampling.cpp │ ├── euclidean_clustering.cpp │ ├── approximate_voxel_grid.cpp │ ├── radius_outlier_removal.cpp │ └── statistical_outlier_removal.cpp ├── common │ ├── cloud_viewer.cpp │ ├── cloud_publisher.cpp │ ├── circular_buffer_pointcloud.cpp │ ├── configurable_object.cpp │ ├── time_utils.cpp │ ├── performance_timer.cpp │ ├── registration_visualizer.cpp │ └── verbosity_levels.cpp ├── localization │ ├── localization.cpp │ └── localization_node.cpp ├── cloud_matchers │ ├── cloud_matcher.cpp │ ├── feature_matchers │ │ ├── keypoint_descriptors │ │ │ ├── pfh.cpp │ │ │ ├── shot.cpp │ │ │ ├── fpfh.cpp │ │ │ ├── esf.cpp │ │ │ ├── spin_image.cpp │ │ │ ├── shape_context_3d.cpp │ │ │ ├── unique_shape_context.cpp │ │ │ ├── keypoint_descriptor.cpp │ │ │ └── keypoint_descriptor_from_normals.cpp │ │ ├── feature_matcher.cpp │ │ ├── keypoint_detectors │ │ │ ├── keypoint_detector.cpp │ │ │ ├── intrinsic_shape_signature_3d.cpp │ │ │ └── sift_3d.cpp │ │ ├── sample_consensus_initial_alignment.cpp │ │ ├── sample_consensus_initial_alignment_prerejective.cpp │ │ ├── ia_ransac.cpp │ │ └── sample_consensus_prerejective.cpp │ └── point_matchers │ │ ├── iterative_closest_point.cpp │ │ ├── iterative_closest_point_2d.cpp │ │ ├── principal_component_analysis.cpp │ │ ├── normal_distributions_transform_2d.cpp │ │ ├── normal_distributions_transform_3d.cpp │ │ ├── iterative_closest_point_non_linear.cpp │ │ ├── iterative_closest_point_generalized.cpp │ │ └── iterative_closest_point_with_normals.cpp ├── cloud_analyzers │ ├── cloud_analyzer.cpp │ └── angular_distribution_analyzer.cpp ├── cluster_selectors │ ├── cluster_selector.cpp │ └── cluster_sorters.cpp ├── normal_estimators │ ├── normal_estimator.cpp │ ├── moving_least_squares.cpp │ ├── normal_estimator_sac.cpp │ └── normal_estimation_omp.cpp ├── outlier_detectors │ ├── outlier_detector.cpp │ └── euclidean_outlier_detector.cpp ├── curvature_estimators │ ├── curvature_estimator.cpp │ └── principal_curvatures_estimation.cpp ├── convergence_estimators │ └── default_convergence_criteria_with_time.cpp └── registration_covariance_estimators │ ├── registration_covariance_estimator.cpp │ ├── registration_covariance_point_to_plane_3d.cpp │ ├── registration_covariance_point_to_point_3d.cpp │ ├── registration_covariance_point_to_plane_pm_3d.cpp │ └── registration_covariance_point_to_point_pm_3d.cpp ├── install ├── d_rosdep.bash ├── install.bash ├── repositories_update.bash ├── e_build.bash ├── b_workspace.bash ├── c_repositories.bash └── a_dependencies.bash ├── include └── dynamic_robot_localization │ ├── common │ ├── time_utils.h │ ├── verbosity_levels.h │ ├── common.h │ ├── performance_timer.h │ ├── cumulative_static_transform_broadcaster.h │ ├── pointcloud_utils.h │ ├── impl │ │ └── math_utils.hpp │ └── math_utils.h │ ├── registration_covariance_estimators │ └── impl │ │ └── registration_covariance_point_to_point_pm_3d.hpp │ └── cloud_analyzers │ └── impl │ └── cloud_analyzer.hpp ├── LICENSE └── package.xml /yaml/configs/empty.yaml: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /srv/StopProcessingSensorData.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool status 3 | -------------------------------------------------------------------------------- /tools/binvox: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carlosmccosta/dynamic_robot_localization/HEAD/tools/binvox -------------------------------------------------------------------------------- /docs/overview.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carlosmccosta/dynamic_robot_localization/HEAD/docs/overview.pdf -------------------------------------------------------------------------------- /docs/overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carlosmccosta/dynamic_robot_localization/HEAD/docs/overview.png -------------------------------------------------------------------------------- /docs/system-modules.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carlosmccosta/dynamic_robot_localization/HEAD/docs/system-modules.png -------------------------------------------------------------------------------- /docs/system-modules.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carlosmccosta/dynamic_robot_localization/HEAD/docs/system-modules.pptx -------------------------------------------------------------------------------- /srv/StartProcessingSensorData.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped initial_pose 2 | bool set_initial_pose 3 | --- 4 | bool status 5 | -------------------------------------------------------------------------------- /docs/perception-overview.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carlosmccosta/dynamic_robot_localization/HEAD/docs/perception-overview.pdf -------------------------------------------------------------------------------- /docs/perception-overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carlosmccosta/dynamic_robot_localization/HEAD/docs/perception-overview.png -------------------------------------------------------------------------------- /rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: C++ API 3 | output_dir: docs/doxygen 4 | file_patterns: '*.cpp *.hpp *.h *.dox' -------------------------------------------------------------------------------- /srv/ReloadLocalizationConfiguration.srv: -------------------------------------------------------------------------------- 1 | dynamic_robot_localization/LocalizationConfiguration localization_configuration 2 | --- 3 | bool status 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | /.pydevproject 15 | cmake-build-debug 16 | .idea 17 | -------------------------------------------------------------------------------- /yaml/configs/disable_debug_topics.yaml: -------------------------------------------------------------------------------- 1 | publish_topic_names: 2 | reference_pointcloud_publish_topic: '' 3 | aligned_pointcloud_publish_topic: '' 4 | pose_with_covariance_stamped_publish_topic: '' 5 | localization_detailed_publish_topic: '' 6 | localization_diagnostics_publish_topic: '' 7 | localization_times_publish_topic: '' 8 | -------------------------------------------------------------------------------- /docs/cad_to_octree.md: -------------------------------------------------------------------------------- 1 | ## [Binvox](http://www.cs.princeton.edu/~min/binvox/) 2 | ``` 3 | binvox -d 1240 -e -fit ship_interior.stl 4 | ``` 5 | 6 | ## [Octree](https://github.com/OctoMap/octomap/wiki/Importing-Data-into-OctoMap) 7 | ``` 8 | binvox2bt --mark-free --bb -5.393 -4.187 -0.00700036 6.607 8.213 2.597 -o ship_interior.bt ship_interior.binvox 9 | ``` 10 | -------------------------------------------------------------------------------- /msg/LocalizationDiagnostics.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint64 number_points_reference_pointcloud 3 | uint64 number_points_reference_pointcloud_after_filtering 4 | uint64 number_keypoints_reference_pointcloud 5 | uint64 number_points_ambient_pointcloud 6 | uint64 number_points_ambient_pointcloud_after_filtering 7 | uint64 number_points_ambient_pointcloud_used_in_registration 8 | uint64 number_keypoints_ambient_pointcloud 9 | int64 number_correspondences_last_registration_algorithm 10 | -------------------------------------------------------------------------------- /yaml/configs/covariance_estimation/covariance_estimation.yaml: -------------------------------------------------------------------------------- 1 | registration_covariance_estimator: 2 | error_metric: 'PointToPlane3D' 3 | correspondance_estimator: 'CorrespondenceEstimation' 4 | correspondence_distance_threshold: 0.1 5 | sensor_std_dev_noise: 0.02 6 | use_reciprocal_correspondences: false 7 | filtered_reference_cloud_publish_topic: 'covariance_reference_cloud' 8 | filtered_ambient_cloud_publish_topic: 'covariance_ambient_cloud' 9 | random_sample: 10 | number_of_random_samples: 250 11 | invert_sampling: false 12 | filtered_cloud_publish_topic: '' 13 | -------------------------------------------------------------------------------- /docs/octomap_commands.md: -------------------------------------------------------------------------------- 1 | ## Save 3D occupancy map 2 | ``` 3 | rosrun octomap_server octomap_saver ~/catkin_ws/src/dynamic_robot_localization_tests/maps/ship_interior/tridimensional/ship_interior_10mm_dynamic.bt 4 | ``` 5 | 6 | ## Save 3D occupancy map with probability information 7 | ``` 8 | rosrun octomap_server octomap_saver -f ~/catkin_ws/src/dynamic_robot_localization_tests/maps/ship_interior/tridimensional/ship_interior_10mm_dynamic.ot 9 | ``` 10 | 11 | ## Save 2D occupancy map 12 | ``` 13 | rosrun map_server map_saver -f ~/catkin_ws/src/dynamic_robot_localization_tests/maps/ship_interior/planar/ship_interior_dynamic map:=projected_map 14 | ``` 15 | -------------------------------------------------------------------------------- /src/transformation_validators/transformation_validator.cpp: -------------------------------------------------------------------------------- 1 | /**\file transformation_validator.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 11 | -------------------------------------------------------------------------------- /msg/LocalizationTimes.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 global_time 3 | float64 filtering_time 4 | float64 surface_normal_estimation_time 5 | float64 keypoint_selection_time 6 | float64 initial_pose_estimation_time 7 | float64 pointcloud_registration_time 8 | float64 correspondence_estimation_time_for_all_matchers 9 | float64 transformation_estimation_time_for_all_matchers 10 | float64 transform_cloud_time_for_all_matchers 11 | float64 cloud_align_time_for_all_matchers 12 | float64 outlier_detection_time 13 | float64 registered_points_angular_distribution_analysis_time 14 | float64 transformation_validators_time 15 | float64 covariance_estimator_time 16 | float64 map_update_time 17 | -------------------------------------------------------------------------------- /install/d_rosdep.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | catkin_ws_path=${1:-"$HOME/catkin_ws_drl"} 4 | ros_version=${2:-"$(rosversion -d)"} 5 | 6 | echo -e "\n\n" 7 | echo "--------------------------------------------------------------------" 8 | echo "--- Installing remaining dependencies" 9 | 10 | cd "${catkin_ws_path}" 11 | rosdep update 12 | rosdep check --from-paths src --ignore-src --rosdistro="${ros_version}" 13 | rosdep install --from-paths src --ignore-src --rosdistro="${ros_version}" 14 | 15 | 16 | echo -e "\n\n" 17 | echo "--------------------------------------------------------------------" 18 | echo "--- Dependencies that must be manually checked" 19 | 20 | rosdep check --from-paths src --ignore-src --rosdistro="${ros_version}" 21 | -------------------------------------------------------------------------------- /msg/LocalizationDetailed.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Vector3 translation_corrections 4 | float64 translation_correction 5 | geometry_msgs/Vector3 rotation_correction_axis 6 | float64 rotation_correction_angle 7 | float64 outlier_percentage 8 | float64 outlier_percentage_reference_pointcloud 9 | float64 root_mean_square_error_inliers 10 | float64 root_mean_square_error_of_last_registration_correspondences 11 | float64 root_mean_square_error_inliers_reference_pointcloud 12 | uint64 number_inliers 13 | uint64 number_inliers_reference_pointcloud 14 | uint64 number_points_registered 15 | float64 inliers_angular_distribution 16 | float64 outliers_angular_distribution 17 | int64 number_of_registration_iterations_for_all_matchers 18 | string last_matcher_convergence_state 19 | geometry_msgs/PoseArray initial_pose_estimation_poses 20 | -------------------------------------------------------------------------------- /yaml/configs/filters/filters_2d.yaml: -------------------------------------------------------------------------------- 1 | filters: 2 | reference_pointcloud: 3 | 1_voxel_grid: 4 | leaf_size_x: 0.005 5 | leaf_size_y: 0.005 6 | leaf_size_z: 0.005 7 | filter_limit_field_name: 'z' 8 | filter_limit_min: -0.001 9 | filter_limit_max: 1.8 10 | filtered_cloud_publish_topic: '' 11 | downsample_all_data: false 12 | save_leaf_layout: false 13 | ambient_pointcloud: 14 | 1_voxel_grid: 15 | leaf_size_x: 0.025 16 | leaf_size_y: 0.025 17 | leaf_size_z: 0.025 18 | filter_limit_field_name: 'z' 19 | filter_limit_min: -1.001 20 | filter_limit_max: 1.8 21 | filtered_cloud_publish_topic: 'ambient_pointcloud_filtered' 22 | downsample_all_data: false 23 | save_leaf_layout: false 24 | -------------------------------------------------------------------------------- /include/dynamic_robot_localization/common/time_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 5 | #include 6 | #include 7 | #include 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | 10 | 11 | namespace dynamic_robot_localization { 12 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 13 | class TimeUtils { 14 | public: 15 | static std::string formatSecondsToDate(double seconds); 16 | }; 17 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 18 | } /* namespace dynamic_robot_localization */ 19 | -------------------------------------------------------------------------------- /include/dynamic_robot_localization/common/verbosity_levels.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /**\file verbosity_levels.h 4 | * \brief Description... 5 | * 6 | * @version 1.0 7 | * @author Carlos Miguel Correia da Costa 8 | */ 9 | 10 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 11 | // std includes 12 | #include 13 | 14 | // ROS includes 15 | #include 16 | 17 | // PCL includes 18 | #include 19 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 20 | 21 | namespace dynamic_robot_localization { 22 | 23 | // ############################################################################ verbosity_levels ############################################################################ 24 | namespace verbosity_levels { 25 | 26 | bool setVerbosityLevelPCL(std::string level); 27 | bool setVerbosityLevelROS(std::string level); 28 | 29 | } /* namespace math_utils */ 30 | } /* namespace dynamic_robot_localization */ 31 | 32 | -------------------------------------------------------------------------------- /yaml/configs/pose_recovery/recovery_3d.yaml: -------------------------------------------------------------------------------- 1 | tracking_recovery_matchers: 2 | point_matchers: 3 | # iterative_closest_point: 4 | # iterative_closest_point_non_linear: 5 | iterative_closest_point_with_normals: 6 | # iterative_closest_point_generalized: 7 | convergence_time_limit_seconds: 0.350 8 | max_correspondence_distance: 5.0 9 | transformation_epsilon: 0.000001 10 | euclidean_fitness_epsilon: 0.000001 11 | max_number_of_registration_iterations: 350 12 | max_number_of_ransac_iterations: 75 13 | ransac_outlier_rejection_threshold: 0.07 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | use_reciprocal_correspondences: false 18 | 19 | 20 | transformation_validators_tracking_recovery: 21 | euclidean_transformation_validator: 22 | max_transformation_angle: 0.7 23 | max_transformation_distance: 0.7 24 | max_new_pose_diff_angle: 1.04 25 | max_new_pose_diff_distance: 0.8 26 | max_root_mean_square_error: 0.07 27 | max_outliers_percentage: 0.66 28 | -------------------------------------------------------------------------------- /yaml/configs/pose_recovery/recovery_3d_large_map.yaml: -------------------------------------------------------------------------------- 1 | tracking_recovery_matchers: 2 | point_matchers: 3 | # iterative_closest_point: 4 | # iterative_closest_point_non_linear: 5 | iterative_closest_point_with_normals: 6 | # iterative_closest_point_generalized: 7 | convergence_time_limit_seconds: 0.450 8 | max_correspondence_distance: 5.0 9 | transformation_epsilon: 0.001 10 | euclidean_fitness_epsilon: 0.001 11 | max_number_of_registration_iterations: 250 12 | max_number_of_ransac_iterations: 150 13 | ransac_outlier_rejection_threshold: 0.07 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | use_reciprocal_correspondences: false 18 | 19 | 20 | transformation_validators_tracking_recovery: 21 | euclidean_transformation_validator: 22 | max_transformation_angle: 0.9 23 | max_transformation_distance: 2.0 24 | max_new_pose_diff_angle: 1.57 25 | max_new_pose_diff_distance: 2.5 26 | max_root_mean_square_error: 0.07 27 | max_outliers_percentage: 0.4 28 | -------------------------------------------------------------------------------- /tools/meshlab_filters.mlx: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/cloud_filters/scale.cpp: -------------------------------------------------------------------------------- 1 | /**\file scale.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_filters/crop_box.cpp: -------------------------------------------------------------------------------- 1 | /**\file crop_box.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/common/cloud_viewer.cpp: -------------------------------------------------------------------------------- 1 | /**\file cloud_viewer.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_filters/voxel_grid.cpp: -------------------------------------------------------------------------------- 1 | /**\file voxel_filter.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /yaml/configs/pose_recovery/recovery_slam_3d.yaml: -------------------------------------------------------------------------------- 1 | tracking_recovery_matchers: 2 | point_matchers: 3 | # iterative_closest_point: 4 | # iterative_closest_point_non_linear: 5 | iterative_closest_point_with_normals: 6 | # iterative_closest_point_generalized: 7 | max_correspondence_distance: 5.0 8 | transformation_epsilon: 0.000001 9 | euclidean_fitness_epsilon: 0.000001 10 | max_number_of_registration_iterations: 350 11 | max_number_of_ransac_iterations: 85 12 | ransac_outlier_rejection_threshold: 0.04 13 | match_only_keypoints: false 14 | display_cloud_aligment: false 15 | maximum_number_of_displayed_correspondences: 0 16 | rotation_epsilon: 0.002 17 | correspondence_randomness: 50 18 | maximum_optimizer_iterations: 100 19 | use_reciprocal_correspondences: false 20 | 21 | 22 | transformation_validators_tracking_recovery: 23 | euclidean_transformation_validator: 24 | max_transformation_angle: 2.09 25 | max_transformation_distance: 1.0 26 | max_new_pose_diff_angle: 3.14 27 | max_new_pose_diff_distance: 2.5 28 | max_root_mean_square_error: 0.045 29 | max_outliers_percentage: 0.55 30 | -------------------------------------------------------------------------------- /src/cloud_filters/cloud_filter.cpp: -------------------------------------------------------------------------------- 1 | /**\file cloud_filter.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_filters/pass_through.cpp: -------------------------------------------------------------------------------- 1 | /**\file voxel_filter.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/localization/localization.cpp: -------------------------------------------------------------------------------- 1 | /**\file localization.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_filters/random_sample.cpp: -------------------------------------------------------------------------------- 1 | /**\file random_sample.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/cloud_matcher.cpp: -------------------------------------------------------------------------------- 1 | /**\file cloud_matcher.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/common/cloud_publisher.cpp: -------------------------------------------------------------------------------- 1 | /**\file cloud_publisher.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_filters/region_growing.cpp: -------------------------------------------------------------------------------- 1 | /**\file region_growing.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_analyzers/cloud_analyzer.cpp: -------------------------------------------------------------------------------- 1 | /**\file cloud_analyzer.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_filters/hsv_segmentation.cpp: -------------------------------------------------------------------------------- 1 | /**\file hsv_segmentation.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cluster_selectors/cluster_selector.cpp: -------------------------------------------------------------------------------- 1 | /**\file cluster_selector.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/normal_estimators/normal_estimator.cpp: -------------------------------------------------------------------------------- 1 | /**\file normal_estimator.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/outlier_detectors/outlier_detector.cpp: -------------------------------------------------------------------------------- 1 | /**\file outlier_detector.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_filters/plane_segmentation.cpp: -------------------------------------------------------------------------------- 1 | /**\file plane_segmentation.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /yaml/configs/pose_recovery/recovery_slam_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_recovery_matchers: 2 | point_matchers: 3 | iterative_closest_point: 4 | # iterative_closest_point_2d: 5 | # iterative_closest_point_non_linear: 6 | # iterative_closest_point_generalized: 7 | convergence_time_limit_seconds: 0.75 8 | max_correspondence_distance: 7.0 9 | transformation_epsilon: 0.000001 10 | euclidean_fitness_epsilon: 0.000001 11 | max_number_of_registration_iterations: 200 12 | max_number_of_ransac_iterations: 100 13 | ransac_outlier_rejection_threshold: 0.04 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | rotation_epsilon: 0.002 18 | correspondence_randomness: 50 19 | maximum_optimizer_iterations: 100 20 | use_reciprocal_correspondences: false 21 | 22 | 23 | transformation_validators_tracking_recovery: 24 | euclidean_transformation_validator: 25 | max_transformation_angle: 2.09 26 | max_transformation_distance: 1.0 27 | max_new_pose_diff_angle: 3.14 28 | max_new_pose_diff_distance: 2.5 29 | max_root_mean_square_error: 0.045 30 | max_outliers_percentage: 0.66 31 | -------------------------------------------------------------------------------- /src/cloud_filters/covariance_sampling.cpp: -------------------------------------------------------------------------------- 1 | /**\file covariance_sampling.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_filters/euclidean_clustering.cpp: -------------------------------------------------------------------------------- 1 | /**\file euclidean_clustering.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/normal_estimators/moving_least_squares.cpp: -------------------------------------------------------------------------------- 1 | /**\file moving_least_squares.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/normal_estimators/normal_estimator_sac.cpp: -------------------------------------------------------------------------------- 1 | /**\file normal_estimator_sac.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /yaml/configs/pose_recovery/recovery_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_recovery_matchers: 2 | point_matchers: 3 | iterative_closest_point: 4 | # iterative_closest_point_2d: 5 | # iterative_closest_point_non_linear: 6 | # iterative_closest_point_generalized: 7 | convergence_time_limit_seconds: 0.75 8 | max_correspondence_distance: 5.0 9 | transformation_epsilon: 0.000001 10 | euclidean_fitness_epsilon: 0.000001 11 | max_number_of_registration_iterations: 200 12 | max_number_of_ransac_iterations: 100 13 | ransac_outlier_rejection_threshold: 0.05 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | # gicp 18 | rotation_epsilon: 0.002 19 | correspondence_randomness: 50 20 | maximum_optimizer_iterations: 100 21 | use_reciprocal_correspondences: false 22 | 23 | 24 | transformation_validators_tracking_recovery: 25 | euclidean_transformation_validator: 26 | max_transformation_angle: 2.09 27 | max_transformation_distance: 1.0 28 | max_new_pose_diff_angle: 3.14 29 | max_new_pose_diff_distance: 2.5 30 | max_root_mean_square_error: 0.05 31 | max_outliers_percentage: 0.66 -------------------------------------------------------------------------------- /src/cloud_filters/approximate_voxel_grid.cpp: -------------------------------------------------------------------------------- 1 | /**\file approximate_voxel_grid.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_filters/radius_outlier_removal.cpp: -------------------------------------------------------------------------------- 1 | /**\file radius_outlier_removal.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/curvature_estimators/curvature_estimator.cpp: -------------------------------------------------------------------------------- 1 | /**\file curvature_estimator.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/normal_estimators/normal_estimation_omp.cpp: -------------------------------------------------------------------------------- 1 | /**\file normal_estimation_omp.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_descriptors/pfh.cpp: -------------------------------------------------------------------------------- 1 | /**\file fpfh.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_descriptors/shot.cpp: -------------------------------------------------------------------------------- 1 | /**\file fpfh.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_descriptors/fpfh.cpp: -------------------------------------------------------------------------------- 1 | /**\file fpfh.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_descriptors/esf.cpp: -------------------------------------------------------------------------------- 1 | /**\file shape_context_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_filters/statistical_outlier_removal.cpp: -------------------------------------------------------------------------------- 1 | /**\file statistical_outlier_removal.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/feature_matcher.cpp: -------------------------------------------------------------------------------- 1 | /**\file feature_matcher.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/outlier_detectors/euclidean_outlier_detector.cpp: -------------------------------------------------------------------------------- 1 | /**\file euclidean_outlier_detector.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/point_matchers/iterative_closest_point.cpp: -------------------------------------------------------------------------------- 1 | /**\file iterative_closest_point.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_analyzers/angular_distribution_analyzer.cpp: -------------------------------------------------------------------------------- 1 | /**\file angular_distribution_analyzer.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /include/dynamic_robot_localization/common/common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /**\file common.h 4 | * \brief Description... 5 | * 6 | * @version 1.0 7 | * @author Carlos Miguel Correia da Costa 8 | */ 9 | 10 | 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | // Define all point types that include PointXYZ and Normal data 13 | #define DRL_POINT_TYPES \ 14 | (pcl::PointXYZRGBNormal) /*\// -> 12 floats 15 | (pcl::PointXYZ) \ // -> 4 floats 16 | (pcl::PointXYZI) \ // -> 8 floats 17 | (pcl::PointXYZRGB) \ // -> 8 floats 18 | (pcl::PointXYZRGBA) \ // -> 8 floats 19 | (pcl::PointNormal) \ // -> 12 floats 20 | (pcl::PointXYZINormal) \ // -> 12 floats*/ 21 | 22 | 23 | 24 | // Define all point types that represent features 25 | #define DRL_DESCRIPTOR_TYPES \ 26 | (pcl::PFHSignature125) \ 27 | (pcl::FPFHSignature33) \ 28 | (pcl::SHOT352) \ 29 | (pcl::ShapeContext1980) \ 30 | (pcl::ESFSignature640) 31 | // (typename pcl::Histogram<153>) 32 | 33 | 34 | #define DRL_SCALAR_TYPES \ 35 | (float) /*\ 36 | (double)*/ 37 | 38 | #define DRL_UNPACK_ARGS( ... ) __VA_ARGS__ 39 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 40 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_detectors/keypoint_detector.cpp: -------------------------------------------------------------------------------- 1 | /**\file keypoint_detector.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_descriptors/spin_image.cpp: -------------------------------------------------------------------------------- 1 | /**\file shape_context_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/point_matchers/iterative_closest_point_2d.cpp: -------------------------------------------------------------------------------- 1 | /**\file iterative_closest_point_2d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_descriptors/shape_context_3d.cpp: -------------------------------------------------------------------------------- 1 | /**\file shape_context_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/point_matchers/principal_component_analysis.cpp: -------------------------------------------------------------------------------- 1 | /**\file principal_component_analysis.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/common/circular_buffer_pointcloud.cpp: -------------------------------------------------------------------------------- 1 | /**\file cloud_publisher.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 23 | -------------------------------------------------------------------------------- /src/curvature_estimators/principal_curvatures_estimation.cpp: -------------------------------------------------------------------------------- 1 | /**\file principal_curvatures_estimation.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_descriptors/unique_shape_context.cpp: -------------------------------------------------------------------------------- 1 | /**\file shape_context_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/point_matchers/normal_distributions_transform_2d.cpp: -------------------------------------------------------------------------------- 1 | /**\file normal_distributions_transform_2d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/point_matchers/normal_distributions_transform_3d.cpp: -------------------------------------------------------------------------------- 1 | /**\file normal_distributions_transform_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_descriptors/keypoint_descriptor.cpp: -------------------------------------------------------------------------------- 1 | /**\file keypoint_descriptor.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/point_matchers/iterative_closest_point_non_linear.cpp: -------------------------------------------------------------------------------- 1 | /**\file iterative_closest_point_non_linear.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 23 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_detectors/intrinsic_shape_signature_3d.cpp: -------------------------------------------------------------------------------- 1 | /**\file intrinsic_shape_signature_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/convergence_estimators/default_convergence_criteria_with_time.cpp: -------------------------------------------------------------------------------- 1 | /**\file default_convergence_criteria_with_time.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/point_matchers/iterative_closest_point_generalized.cpp: -------------------------------------------------------------------------------- 1 | /**\file iterative_closest_point_generalized.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 23 | -------------------------------------------------------------------------------- /src/cloud_matchers/point_matchers/iterative_closest_point_with_normals.cpp: -------------------------------------------------------------------------------- 1 | /**\file iterative_closest_point_with_normals.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 23 | -------------------------------------------------------------------------------- /src/registration_covariance_estimators/registration_covariance_estimator.cpp: -------------------------------------------------------------------------------- 1 | /**\file registration_registration_covariance_estimator.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /install/install.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | catkin_ws_path=${1:-"$HOME/catkin_ws_drl"} 4 | number_of_cpu_threads_for_compilation=${2:-2} 5 | use_catkin_tools=${3:-true} 6 | ros_version=${4:-"$(rosversion -d)"} 7 | catkin_ws_path_to_extend=${5:-"/opt/ros/$ros_version"} 8 | 9 | 10 | echo -e "\n\n" 11 | echo "####################################################################################################" 12 | echo "##### Installing the dynamic_robot_localization ROS package..." 13 | echo "===== catkin_ws_path: ${catkin_ws_path}" 14 | echo "===== number_of_cpu_threads_for_compilation: ${number_of_cpu_threads_for_compilation}" 15 | echo "===== use_catkin_tools: ${use_catkin_tools}" 16 | echo "===== ros_version: ${ros_version}" 17 | echo "===== catkin_ws_path_to_extend: ${catkin_ws_path_to_extend}" 18 | echo -e "####################################################################################################\n" 19 | 20 | script_dir="$(dirname "$(readlink -e "${BASH_SOURCE[0]}")" && echo X)" && script_dir="${script_dir%$'\nX'}" 21 | 22 | "${script_dir}/a_dependencies.bash" "${ros_version}" 23 | "${script_dir}/b_workspace.bash" "${catkin_ws_path}" "${use_catkin_tools}" "${ros_version}" "${catkin_ws_path_to_extend}" 24 | "${script_dir}/c_repositories.bash" "${catkin_ws_path}" 25 | "${script_dir}/d_rosdep.bash" "${catkin_ws_path}" "${ros_version}" 26 | "${script_dir}/e_build.bash" "${catkin_ws_path}" "${number_of_cpu_threads_for_compilation}" "${use_catkin_tools}" "${ros_version}" "${catkin_ws_path_to_extend}" 27 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/sample_consensus_initial_alignment.cpp: -------------------------------------------------------------------------------- 1 | /**\file sample_consensus_initial_alignment.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/cluttered_environments_3d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | convergence_time_limit_seconds: 0.250 9 | max_correspondence_distance: 0.05 10 | transformation_epsilon: 0.0001 11 | euclidean_fitness_epsilon: 0.0001 12 | max_number_of_registration_iterations: 350 13 | max_number_of_ransac_iterations: 50 14 | ransac_outlier_rejection_threshold: 0.05 15 | match_only_keypoints: false 16 | display_cloud_aligment: false 17 | maximum_number_of_displayed_correspondences: 0 18 | use_reciprocal_correspondences: false 19 | 20 | 21 | outlier_detectors: 22 | euclidean_outlier_detector: 23 | max_inliers_distance: 0.04 24 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 25 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 26 | 27 | 28 | transformation_validators: 29 | euclidean_transformation_validator: 30 | max_transformation_angle: 0.4 31 | max_transformation_distance: 0.035 32 | max_new_pose_diff_angle: 0.6 33 | max_new_pose_diff_distance: 0.1 34 | max_root_mean_square_error: 0.07 35 | max_outliers_percentage: 0.8 36 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/unstable_ground_3d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | # iterative_closest_point_non_linear: 6 | iterative_closest_point_with_normals: 7 | # iterative_closest_point_generalized: 8 | convergence_time_limit_seconds: 0.250 9 | max_correspondence_distance: 0.25 10 | transformation_epsilon: 0.0001 11 | euclidean_fitness_epsilon: 0.0001 12 | max_number_of_registration_iterations: 350 13 | max_number_of_ransac_iterations: 75 14 | ransac_outlier_rejection_threshold: 0.03 15 | match_only_keypoints: false 16 | display_cloud_aligment: false 17 | maximum_number_of_displayed_correspondences: 0 18 | use_reciprocal_correspondences: false 19 | 20 | 21 | outlier_detectors: 22 | euclidean_outlier_detector: 23 | max_inliers_distance: 0.07 24 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 25 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 26 | 27 | 28 | transformation_validators: 29 | euclidean_transformation_validator: 30 | max_transformation_angle: 0.4 31 | max_transformation_distance: 0.5 32 | max_new_pose_diff_angle: 0.6 33 | max_new_pose_diff_distance: 1.0 34 | max_root_mean_square_error: 0.07 35 | max_outliers_percentage: 0.5 36 | -------------------------------------------------------------------------------- /src/registration_covariance_estimators/registration_covariance_point_to_plane_3d.cpp: -------------------------------------------------------------------------------- 1 | /**\file registration_covariance_point_to_plane_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/registration_covariance_estimators/registration_covariance_point_to_point_3d.cpp: -------------------------------------------------------------------------------- 1 | /**\file registration_covariance_point_to_point_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/cluttered_environments_dynamic_3d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | iterative_closest_point: 5 | # iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | convergence_time_limit_seconds: 0.250 9 | max_correspondence_distance: 0.05 10 | transformation_epsilon: 0.0001 11 | euclidean_fitness_epsilon: 0.0001 12 | max_number_of_registration_iterations: 350 13 | max_number_of_ransac_iterations: 50 14 | ransac_outlier_rejection_threshold: 0.05 15 | match_only_keypoints: false 16 | display_cloud_aligment: false 17 | maximum_number_of_displayed_correspondences: 0 18 | use_reciprocal_correspondences: false 19 | 20 | 21 | outlier_detectors: 22 | euclidean_outlier_detector: 23 | max_inliers_distance: 0.07 24 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 25 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 26 | 27 | 28 | transformation_validators: 29 | euclidean_transformation_validator: 30 | max_transformation_angle: 0.4 31 | max_transformation_distance: 0.035 32 | max_new_pose_diff_angle: 0.6 33 | max_new_pose_diff_distance: 0.1 34 | max_root_mean_square_error: 0.1 35 | max_outliers_percentage: 0.75 36 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_descriptors/keypoint_descriptor_from_normals.cpp: -------------------------------------------------------------------------------- 1 | /**\file keypoint_descriptor.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/registration_covariance_estimators/registration_covariance_point_to_plane_pm_3d.cpp: -------------------------------------------------------------------------------- 1 | /**\file registration_covariance_point_to_plane_pm_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/registration_covariance_estimators/registration_covariance_point_to_point_pm_3d.cpp: -------------------------------------------------------------------------------- 1 | /**\file registration_covariance_point_to_point_pm_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/sample_consensus_initial_alignment_prerejective.cpp: -------------------------------------------------------------------------------- 1 | /**\file sample_consensus_initial_alignment.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | -------------------------------------------------------------------------------- /msg/LocalizationConfiguration.msg: -------------------------------------------------------------------------------- 1 | # Below are the parameter server namespaces that can be provided for reloading each group of the drl processing pipeline (omit leading "/" for using the node local namespace) 2 | # If a given string is empty, the reloading of the associated modules will be skipped 3 | # The drl by default uses a empty configuration_namespace because it loads the configurations from the root of the node local namespace (~) 4 | # As such, use "~" to inform that you want to reload modules from the root of the node local namespace (if a custom namespace is given, the "~" character is not needed since drl uses a private node handle to load its configurations) 5 | string message_management 6 | string general_configurations 7 | string subscribe_topic_names 8 | string service_servers_names 9 | string publish_topic_names 10 | string frame_ids 11 | string reference_pointcloud 12 | string filters 13 | string normal_estimators 14 | string curvature_estimators 15 | string keypoint_detectors 16 | string cloud_matchers_configurations 17 | string initial_pose_estimators_feature_matchers 18 | string initial_pose_estimators_point_matchers 19 | string tracking_matchers 20 | string tracking_recovery_matchers 21 | string transformation_aligner 22 | string outlier_detectors 23 | string outlier_detectors_reference_pointcloud 24 | string cloud_analyzers 25 | string transformation_validators_for_initial_alignment 26 | string transformation_validators_for_tracking 27 | string transformation_validators_for_tracking_recovery 28 | string registration_covariance_estimators 29 | string tf_publisher 30 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Carlos Miguel Correia da Costa 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the {organization} nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /yaml/configs/filters/filters_3d.yaml: -------------------------------------------------------------------------------- 1 | filters: 2 | # reference_pointcloud: 3 | # 1_pass_through_x: 4 | # field_name: 'x' 5 | # min_value: -5.392 6 | # max_value: 3.0069 7 | # filtered_cloud_publish_topic: '' 8 | # 2_pass_through_y: 9 | # field_name: 'y' 10 | # min_value: -4.186 11 | # max_value: 8.212 12 | # filtered_cloud_publish_topic: '' 13 | # 3_pass_through_z: 14 | # field_name: 'z' 15 | # min_value: -0.001 16 | # max_value: 1.8 17 | # filtered_cloud_publish_topic: '' 18 | # 3_voxel_grid: 19 | # leaf_size_x: 0.02 20 | # leaf_size_y: 0.02 21 | # leaf_size_z: 0.02 22 | # filter_limit_field_name: 'z' 23 | # filter_limit_min: -0.001 24 | # filter_limit_max: 1.8 25 | # filtered_cloud_publish_topic: '' 26 | # downsample_all_data: false 27 | # save_leaf_layout: false 28 | ambient_pointcloud: 29 | 1_voxel_grid: 30 | leaf_size_x: 0.025 31 | leaf_size_y: 0.025 32 | leaf_size_z: 0.025 33 | filter_limit_field_name: 'z' 34 | filter_limit_min: -5.0 35 | filter_limit_max: 5.0 36 | filtered_cloud_publish_topic: 'ambient_pointcloud_filtered' 37 | downsample_all_data: false 38 | save_leaf_layout: false 39 | 2_random_sample: 40 | number_of_random_samples: 500 41 | invert_sampling: false 42 | filtered_cloud_publish_topic: '' 43 | -------------------------------------------------------------------------------- /yaml/configs/filters/filters_3d_slam.yaml: -------------------------------------------------------------------------------- 1 | filters: 2 | reference_pointcloud: 3 | # 1_pass_through_x: 4 | # field_name: 'x' 5 | # min_value: -5.392 6 | # max_value: 3.0069 7 | # filtered_cloud_publish_topic: '' 8 | # 2_pass_through_y: 9 | # field_name: 'y' 10 | # min_value: -4.186 11 | # max_value: 8.212 12 | # filtered_cloud_publish_topic: '' 13 | # 3_pass_through_z: 14 | # field_name: 'z' 15 | # min_value: -0.001 16 | # max_value: 1.8 17 | # filtered_cloud_publish_topic: '' 18 | 3_voxel_grid: 19 | leaf_size_x: 0.025 20 | leaf_size_y: 0.025 21 | leaf_size_z: 0.025 22 | filter_limit_field_name: 'z' 23 | filter_limit_min: -0.05 24 | filter_limit_max: 2.5 25 | filtered_cloud_publish_topic: '' 26 | downsample_all_data: false 27 | save_leaf_layout: false 28 | ambient_pointcloud: 29 | 1_voxel_grid: 30 | leaf_size_x: 0.025 31 | leaf_size_y: 0.025 32 | leaf_size_z: 0.025 33 | filter_limit_field_name: 'z' 34 | filter_limit_min: -5.0 35 | filter_limit_max: 5.0 36 | filtered_cloud_publish_topic: 'ambient_pointcloud_filtered' 37 | downsample_all_data: false 38 | save_leaf_layout: false 39 | # 2_random_sample: 40 | # number_of_random_samples: 500 41 | # invert_sampling: false 42 | # filtered_cloud_publish_topic: '' 43 | -------------------------------------------------------------------------------- /src/common/configurable_object.cpp: -------------------------------------------------------------------------------- 1 | /**\file configurable_object.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 11 | 12 | namespace dynamic_robot_localization { 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 15 | bool ConfigurableObject::s_parseConfigurationNamespaceFromParameterServer(const std::string& configuration_namespace, std::string& configuration_namespace_parsed_out) { 16 | configuration_namespace_parsed_out.clear(); 17 | 18 | if (configuration_namespace.empty()) 19 | return false; 20 | 21 | if (configuration_namespace == "~") { 22 | return true; 23 | } 24 | 25 | configuration_namespace_parsed_out = configuration_namespace; 26 | if (configuration_namespace.back() != '/') { 27 | configuration_namespace_parsed_out += '/'; 28 | } 29 | 30 | return true; 31 | } 32 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 33 | 34 | } /* namespace dynamic_robot_localization */ 35 | -------------------------------------------------------------------------------- /src/common/time_utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace dynamic_robot_localization { 4 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 5 | std::string TimeUtils::formatSecondsToDate(double seconds) { 6 | double secondsFinal = fmod(seconds, 60); 7 | 8 | double minutes = seconds / 60.0; 9 | unsigned short minutesFinal = (unsigned short) fmod(minutes, 60.0); 10 | 11 | double hours = minutes / 60; 12 | unsigned short hoursFinal = (unsigned short) fmod(hours, 24.0); 13 | 14 | unsigned short daysFinal = (unsigned short) (hours / 24.0); 15 | 16 | std::stringstream timeFormated; 17 | if (daysFinal != 0) { 18 | timeFormated << daysFinal << "d"; 19 | } 20 | 21 | if (hoursFinal != 0 || daysFinal != 0) { 22 | if (hoursFinal < 10) { 23 | timeFormated << 0; 24 | } 25 | timeFormated << hoursFinal << "h"; 26 | } 27 | 28 | if (minutesFinal != 0 || hoursFinal != 0 || daysFinal != 0) { 29 | if (minutesFinal < 10) { 30 | timeFormated << 0; 31 | } 32 | timeFormated << minutesFinal << "m"; 33 | } 34 | 35 | if ((minutesFinal != 0 || hoursFinal != 0 || daysFinal != 0) && secondsFinal < 10 && secondsFinal >= 1) { 36 | timeFormated << 0; 37 | } 38 | timeFormated << secondsFinal << "s"; 39 | 40 | return timeFormated.str(); 41 | } 42 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 43 | } /* namespace dynamic_robot_localization */ 44 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/unstable_ground_3d_tof.yaml: -------------------------------------------------------------------------------- 1 | message_management: 2 | maximum_number_points_ambient_pointcloud_circular_buffer: 500 3 | 4 | 5 | tracking_matchers: 6 | ignore_height_corrections: false 7 | point_matchers: 8 | iterative_closest_point: 9 | # iterative_closest_point_non_linear: 10 | # iterative_closest_point_with_normals: 11 | # iterative_closest_point_generalized: 12 | convergence_time_limit_seconds: 0.500 13 | max_correspondence_distance: 0.35 14 | transformation_epsilon: 0.001 15 | euclidean_fitness_epsilon: 0.001 16 | max_number_of_registration_iterations: 150 17 | max_number_of_ransac_iterations: 50 18 | ransac_outlier_rejection_threshold: 0.05 19 | match_only_keypoints: false 20 | display_cloud_aligment: false 21 | maximum_number_of_displayed_correspondences: 0 22 | rotation_epsilon: 0.002 23 | correspondence_randomness: 50 24 | maximum_optimizer_iterations: 100 25 | use_reciprocal_correspondences: false 26 | 27 | 28 | outlier_detectors: 29 | euclidean_outlier_detector: 30 | max_inliers_distance: 0.06 31 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 32 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 33 | 34 | 35 | transformation_validators: 36 | euclidean_transformation_validator: 37 | max_transformation_angle: 0.3 38 | max_transformation_distance: 0.2 39 | max_new_pose_diff_angle: 1.0 40 | max_new_pose_diff_distance: 1.0 41 | max_root_mean_square_error: 0.07 42 | max_outliers_percentage: 0.4 43 | -------------------------------------------------------------------------------- /install/repositories_update.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | reset_ws=${1:-true} 4 | catkin_ws_path=${2:-"$HOME/catkin_ws_drl"} 5 | repositories_dirs=${3:-"dynamic_robot_localization pose_to_tf_publisher laserscan_to_pointcloud mesh_to_pointcloud pcl pcl_msgs perception_pcl"} 6 | 7 | 8 | echo -e "\n\n" 9 | echo "####################################################################################################" 10 | echo "##### Updating git repositories..." 11 | if [ "${reset_ws}" = true ] ; then 12 | echo "##### git reset --hard HEAD will be performed before pulling" 13 | fi 14 | echo -e "####################################################################################################\n" 15 | 16 | 17 | cd "${catkin_ws_path}/src/" &> /dev/null 18 | if [ $? -ne 0 ]; then 19 | echo "===============================================================" 20 | echo -e "==> Missing workspace directory (${catkin_ws_path}/src/)!\n\n" 21 | exit -1 22 | fi 23 | 24 | 25 | for git_repository in ${repositories_dirs} 26 | do 27 | cd "${git_repository}" &> /dev/null 28 | if [ $? -eq 0 ]; then 29 | echo -e "\n\n-------------------------------------------" 30 | echo "==> Updating ${git_repository}" 31 | if [ "${reset_ws}" = true ] ; then 32 | git reset --hard HEAD 33 | fi 34 | git pull 35 | cd .. 36 | fi 37 | done 38 | 39 | cd "${catkin_ws_path}" 40 | find ./src -name "*.bash" -exec chmod +x {} \; 41 | find ./src -name "*.cfg" -exec chmod +x {} \; 42 | find ./src -name "*.sh" -exec chmod +x {} \; 43 | 44 | 45 | echo -e "\n\n" 46 | echo "----------------------------------------------------------------------------------------------------" 47 | echo ">>>>> Update finished" 48 | echo "----------------------------------------------------------------------------------------------------" 49 | 50 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/keypoint_detectors/sift_3d.cpp: -------------------------------------------------------------------------------- 1 | /**\file sift_3d.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 42 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/static_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | max_correspondence_distance: 0.05 9 | transformation_epsilon: 0.0001 10 | euclidean_fitness_epsilon: 0.0001 11 | max_number_of_registration_iterations: 50 12 | max_number_of_ransac_iterations: 50 13 | ransac_outlier_rejection_threshold: 0.05 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | use_reciprocal_correspondences: false 18 | 19 | 20 | outlier_detectors: 21 | euclidean_outlier_detector: 22 | max_inliers_distance: 0.04 23 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 24 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 25 | 26 | 27 | cloud_analyzers: 28 | compute_inliers_angular_distribution: true 29 | compute_outliers_angular_distribution: true 30 | angular_distribution_analyzer: 31 | number_of_angular_bins: 180 32 | 33 | 34 | transformation_validators: 35 | euclidean_transformation_validator: 36 | max_transformation_angle: 0.5 37 | max_transformation_distance: 0.05 38 | max_new_pose_diff_angle: 1.0 39 | max_new_pose_diff_distance: 0.1 40 | max_root_mean_square_error: 0.025 41 | max_outliers_percentage: 0.05 42 | 43 | 44 | transformation_validators_tracking_recovery: 45 | euclidean_transformation_validator: 46 | max_root_mean_square_error: 0.05 47 | max_outliers_percentage: 0.1 48 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/static_fast_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | max_correspondence_distance: 0.15 9 | transformation_epsilon: 0.0001 10 | euclidean_fitness_epsilon: 0.0001 11 | max_number_of_registration_iterations: 50 12 | max_number_of_ransac_iterations: 25 13 | ransac_outlier_rejection_threshold: 0.05 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | use_reciprocal_correspondences: false 18 | 19 | 20 | outlier_detectors: 21 | euclidean_outlier_detector: 22 | max_inliers_distance: 0.1 23 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 24 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 25 | 26 | 27 | cloud_analyzers: 28 | compute_inliers_angular_distribution: true 29 | compute_outliers_angular_distribution: true 30 | angular_distribution_analyzer: 31 | number_of_angular_bins: 180 32 | 33 | 34 | transformation_validators: 35 | euclidean_transformation_validator: 36 | max_transformation_angle: 0.7 37 | max_transformation_distance: 0.15 38 | max_new_pose_diff_angle: 1.0 39 | max_new_pose_diff_distance: 0.2 40 | max_root_mean_square_error: 0.1 41 | max_outliers_percentage: 0.1 42 | 43 | 44 | transformation_validators_tracking_recovery: 45 | euclidean_transformation_validator: 46 | max_root_mean_square_error: 0.15 47 | max_outliers_percentage: 0.15 48 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/unstable_ground_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | max_correspondence_distance: 0.05 9 | transformation_epsilon: 0.0001 10 | euclidean_fitness_epsilon: 0.0001 11 | max_number_of_registration_iterations: 250 12 | max_number_of_ransac_iterations: 50 13 | ransac_outlier_rejection_threshold: 0.05 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | use_reciprocal_correspondences: false 18 | 19 | 20 | outlier_detectors: 21 | euclidean_outlier_detector: 22 | max_inliers_distance: 0.1 23 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 24 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 25 | 26 | 27 | cloud_analyzers: 28 | compute_inliers_angular_distribution: true 29 | compute_outliers_angular_distribution: true 30 | angular_distribution_analyzer: 31 | number_of_angular_bins: 180 32 | 33 | 34 | transformation_validators: 35 | euclidean_transformation_validator: 36 | max_transformation_angle: 4 37 | max_transformation_distance: 0.035 38 | max_new_pose_diff_angle: 8 39 | max_new_pose_diff_distance: 0.2 40 | max_root_mean_square_error: 0.025 41 | max_outliers_percentage: 0.07 42 | 43 | 44 | transformation_validators_tracking_recovery: 45 | euclidean_transformation_validator: 46 | max_root_mean_square_error: 0.05 47 | max_outliers_percentage: 0.1 48 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/cluttered_environments_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | max_correspondence_distance: 0.02 9 | transformation_epsilon: 0.0001 10 | euclidean_fitness_epsilon: 0.0001 11 | max_number_of_registration_iterations: 75 12 | max_number_of_ransac_iterations: 75 13 | ransac_outlier_rejection_threshold: 0.05 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | use_reciprocal_correspondences: false 18 | 19 | 20 | outlier_detectors: 21 | euclidean_outlier_detector: 22 | max_inliers_distance: 0.03 23 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 24 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 25 | 26 | 27 | cloud_analyzers: 28 | compute_inliers_angular_distribution: true 29 | compute_outliers_angular_distribution: true 30 | angular_distribution_analyzer: 31 | number_of_angular_bins: 180 32 | 33 | 34 | transformation_validators: 35 | euclidean_transformation_validator: 36 | max_transformation_angle: 0.4 37 | max_transformation_distance: 0.02 38 | max_new_pose_diff_angle: 0.6 39 | max_new_pose_diff_distance: 0.1 40 | max_root_mean_square_error: 0.07 41 | max_outliers_percentage: 0.70 42 | 43 | 44 | transformation_validators_tracking_recovery: 45 | euclidean_transformation_validator: 46 | max_root_mean_square_error: 0.07 47 | max_outliers_percentage: 0.75 48 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/cluttered_environments_fast_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | max_correspondence_distance: 0.2 9 | transformation_epsilon: 0.0001 10 | euclidean_fitness_epsilon: 0.0001 11 | max_number_of_registration_iterations: 75 12 | max_number_of_ransac_iterations: 25 13 | ransac_outlier_rejection_threshold: 0.05 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | use_reciprocal_correspondences: false 18 | 19 | 20 | outlier_detectors: 21 | euclidean_outlier_detector: 22 | max_inliers_distance: 0.1 23 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 24 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 25 | 26 | 27 | cloud_analyzers: 28 | compute_inliers_angular_distribution: true 29 | compute_outliers_angular_distribution: true 30 | angular_distribution_analyzer: 31 | number_of_angular_bins: 180 32 | 33 | 34 | transformation_validators: 35 | euclidean_transformation_validator: 36 | max_transformation_angle: 0.8 37 | max_transformation_distance: 0.17 38 | max_new_pose_diff_angle: 1.2 39 | max_new_pose_diff_distance: 0.25 40 | max_root_mean_square_error: 0.2 41 | max_outliers_percentage: 0.70 42 | 43 | 44 | transformation_validators_tracking_recovery: 45 | euclidean_transformation_validator: 46 | max_root_mean_square_error: 0.20 47 | max_outliers_percentage: 0.75 48 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/static_without_odometry_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | max_correspondence_distance: 0.5 9 | transformation_epsilon: 0.0001 10 | euclidean_fitness_epsilon: 0.0001 11 | max_number_of_registration_iterations: 250 12 | max_number_of_ransac_iterations: 50 13 | ransac_outlier_rejection_threshold: 0.05 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | use_reciprocal_correspondences: false 18 | 19 | 20 | outlier_detectors: 21 | euclidean_outlier_detector: 22 | max_inliers_distance: 0.1 23 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 24 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 25 | 26 | 27 | cloud_analyzers: 28 | compute_inliers_angular_distribution: true 29 | compute_outliers_angular_distribution: true 30 | angular_distribution_analyzer: 31 | number_of_angular_bins: 180 32 | 33 | 34 | transformation_validators: 35 | euclidean_transformation_validator: 36 | max_transformation_angle: 0.79 37 | max_transformation_distance: 0.5 38 | max_new_pose_diff_angle: 1.0 39 | max_new_pose_diff_distance: 1.0 40 | max_root_mean_square_error: 0.15 41 | max_outliers_percentage: 0.1 42 | 43 | 44 | transformation_validators_tracking_recovery: 45 | euclidean_transformation_validator: 46 | max_root_mean_square_error: 0.15 47 | max_outliers_percentage: 0.15 48 | -------------------------------------------------------------------------------- /tools/create_eclipse_projects.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | echo "####################################################################################################" 4 | echo "##### Creating eclipse project for package dynamic_robot_localization" 5 | echo "####################################################################################################" 6 | 7 | 8 | mkdir -p ~/catkin_ws/build/dynamic_robot_localization 9 | cd ~/catkin_ws/build/dynamic_robot_localization 10 | cmake -G"Eclipse CDT4 - Unix Makefiles" -DCMAKE_ECLIPSE_GENERATE_SOURCE_PROJECT=TRUE -DCMAKE_BUILD_TYPE=Debug -DCMAKE_ECLIPSE_MAKE_ARGUMENTS=-j8 ~/catkin_ws/src/dynamic_robot_localization 11 | 12 | 13 | echo -e "\n\n" 14 | echo "----------------------------------------------------------------------------------------------------" 15 | echo ">>>>> Eclipse build project in ~/catkin_ws/build/dynamic_robot_localization" 16 | echo ">>>>> Eclipse source project in ~/catkin_ws/src/dynamic_robot_localization" 17 | echo "----------------------------------------------------------------------------------------------------" 18 | 19 | 20 | 21 | echo -e "\n\n\n\n" 22 | echo "####################################################################################################" 23 | echo "##### Creating eclipse projects for catkin workspace" 24 | echo "####################################################################################################" 25 | 26 | cd ~/catkin_ws 27 | catkin_make --force-cmake -G"Eclipse CDT4 - Unix Makefiles" -DCMAKE_BUILD_TYPE=Debug -DCMAKE_ECLIPSE_MAKE_ARGUMENTS=-j8 28 | 29 | 30 | echo -e "\n\n" 31 | echo "----------------------------------------------------------------------------------------------------" 32 | echo ">>>>> Eclipse catkin_ws project in ~/catkin_ws/build" 33 | echo "----------------------------------------------------------------------------------------------------" 34 | -------------------------------------------------------------------------------- /src/cluster_selectors/cluster_sorters.cpp: -------------------------------------------------------------------------------- 1 | /**\file cluster_sorters.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | #include 11 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 12 | 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 33 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/static_3d.yaml: -------------------------------------------------------------------------------- 1 | #message_management: 2 | # maximum_number_points_ambient_pointcloud_circular_buffer: 750 3 | 4 | 5 | #normal_estimators: 6 | # reference_pointcloud: 7 | # display_normals: true 8 | # normal_estimation_omp: 9 | # search_k: 7 10 | # search_radius: 0.12 11 | 12 | 13 | tracking_matchers: 14 | ignore_height_corrections: false 15 | point_matchers: 16 | iterative_closest_point: 17 | # iterative_closest_point_non_linear: 18 | # iterative_closest_point_with_normals: 19 | # iterative_closest_point_generalized: 20 | convergence_time_limit_seconds: 0.250 21 | max_correspondence_distance: 0.1 22 | transformation_epsilon: 0.0001 23 | euclidean_fitness_epsilon: 0.0001 24 | max_number_of_registration_iterations: 250 25 | max_number_of_ransac_iterations: 50 26 | ransac_outlier_rejection_threshold: 0.1 27 | match_only_keypoints: false 28 | display_cloud_aligment: false 29 | maximum_number_of_displayed_correspondences: 0 30 | rotation_epsilon: 0.002 31 | correspondence_randomness: 50 32 | maximum_optimizer_iterations: 100 33 | use_reciprocal_correspondences: false 34 | 35 | 36 | outlier_detectors: 37 | euclidean_outlier_detector: 38 | max_inliers_distance: 0.04 39 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 40 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 41 | 42 | 43 | transformation_validators: 44 | euclidean_transformation_validator: 45 | max_transformation_angle: 0.3 46 | max_transformation_distance: 0.035 47 | max_new_pose_diff_angle: 1.0 48 | max_new_pose_diff_distance: 0.5 49 | max_root_mean_square_error: 0.025 50 | max_outliers_percentage: 0.05 51 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/cluttered_environments_dynamic_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | max_correspondence_distance: 0.05 9 | transformation_epsilon: 0.0001 10 | euclidean_fitness_epsilon: 0.0001 11 | max_number_of_registration_iterations: 75 12 | max_number_of_ransac_iterations: 75 13 | ransac_outlier_rejection_threshold: 0.05 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | rotation_epsilon: 0.002 18 | correspondence_randomness: 50 19 | maximum_optimizer_iterations: 100 20 | use_reciprocal_correspondences: false 21 | 22 | 23 | outlier_detectors: 24 | euclidean_outlier_detector: 25 | max_inliers_distance: 0.05 26 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 27 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 28 | 29 | 30 | cloud_analyzers: 31 | compute_inliers_angular_distribution: true 32 | compute_outliers_angular_distribution: true 33 | angular_distribution_analyzer: 34 | number_of_angular_bins: 180 35 | 36 | 37 | transformation_validators: 38 | euclidean_transformation_validator: 39 | max_transformation_angle: 0.4 40 | max_transformation_distance: 0.035 41 | max_new_pose_diff_angle: 0.6 42 | max_new_pose_diff_distance: 0.1 43 | max_root_mean_square_error: 0.1 44 | max_outliers_percentage: 0.70 45 | 46 | 47 | transformation_validators_tracking_recovery: 48 | euclidean_transformation_validator: 49 | max_root_mean_square_error: 0.1 50 | max_outliers_percentage: 0.75 51 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/cluttered_environments_dynamic_fast_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | max_correspondence_distance: 0.2 9 | transformation_epsilon: 0.0001 10 | euclidean_fitness_epsilon: 0.0001 11 | max_number_of_registration_iterations: 75 12 | max_number_of_ransac_iterations: 25 13 | ransac_outlier_rejection_threshold: 0.05 14 | match_only_keypoints: false 15 | display_cloud_aligment: false 16 | maximum_number_of_displayed_correspondences: 0 17 | rotation_epsilon: 0.002 18 | correspondence_randomness: 50 19 | maximum_optimizer_iterations: 100 20 | use_reciprocal_correspondences: false 21 | 22 | 23 | outlier_detectors: 24 | euclidean_outlier_detector: 25 | max_inliers_distance: 0.09 26 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 27 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 28 | 29 | 30 | cloud_analyzers: 31 | compute_inliers_angular_distribution: true 32 | compute_outliers_angular_distribution: true 33 | angular_distribution_analyzer: 34 | number_of_angular_bins: 180 35 | 36 | 37 | transformation_validators: 38 | euclidean_transformation_validator: 39 | max_transformation_angle: 0.9 40 | max_transformation_distance: 0.2 41 | max_new_pose_diff_angle: 1.3 42 | max_new_pose_diff_distance: 0.3 43 | max_root_mean_square_error: 0.25 44 | max_outliers_percentage: 0.70 45 | 46 | 47 | transformation_validators_tracking_recovery: 48 | euclidean_transformation_validator: 49 | max_root_mean_square_error: 0.25 50 | max_outliers_percentage: 0.75 51 | -------------------------------------------------------------------------------- /tools/convert_mesh_to_pcd.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | ################################################## 4 | ################### parameters ################### 5 | ################################################## 6 | 7 | input_file=${1:?'Must specify file to convert'} 8 | conversion_extension=${2:-'.ply'} 9 | meshlab_filters=${3:-'meshlab_filters.mlx'} 10 | meshlab_filters_options=${4:-'-om vq vn'} #-om vc vq vn 11 | final_extension=${5:-'.pcd'} 12 | 13 | 14 | 15 | echo "####################################################################################################" 16 | echo "##### Converting ${input_file} into ${final_extension}" 17 | echo "####################################################################################################\n" 18 | 19 | 20 | ######### convert file 21 | file_without_extension="${input_file%.*}" 22 | stdout_log="${file_without_extension}__stdout.log" 23 | stderr_log="${file_without_extension}__stderr.log" 24 | conversion_file="${file_without_extension}${conversion_extension}" 25 | output_file="${file_without_extension}${final_extension}" 26 | 27 | echo "==> Filtering ${input_file} into ${conversion_file} using meshlab" 28 | meshlabserver -i ${input_file} -o ${conversion_file} -s ${meshlab_filters} ${meshlab_filters_options} > ${stdout_log} 2> ${stderr_log} 29 | 30 | if [ "${conversion_extension}" = '.ply' ]; then 31 | echo " +> Changing ply file to have curvature field instead of quality" 32 | sed -i 's/quality/curvature/' ${conversion_file} > ${stdout_log} 2> ${stderr_log} 33 | fi 34 | 35 | echo " +> Converting ${conversion_file} into ${output_file} using drl_mesh_to_pcd" 36 | rosrun dynamic_robot_localization drl_mesh_to_pcd ${conversion_file} ${output_file} -binary 1 -compressed 1 -type auto > ${stdout_log} 2> ${stderr_log} 37 | echo " !> Finished conversion of ${conversion_file} into ${final_extension}\n" 38 | 39 | 40 | echo "----------------------------------------------------------------------------------------------------" 41 | echo ">>>>> Finished mesh conversion." 42 | echo "----------------------------------------------------------------------------------------------------" 43 | -------------------------------------------------------------------------------- /docs/mesh_conversions.md: -------------------------------------------------------------------------------- 1 | # mesh conversions 2 | 3 | ## convert a mesh using tools/convert_mesh_to_pcd.sh 4 | ``` 5 | convert_mesh_to_pcd.sh [conversion_extension] [meshlab_filters] [meshlab_filters_options] [final_extension] 6 | convert_mesh_to_pcd.sh input.ply 7 | ``` 8 | 9 | ## convert all meshes inside a folder with a given extension using tools/convert_meshes_to_pcd.sh 10 | ``` 11 | convert_meshes_to_pcd.sh [search_path] [search_extension] [max_parallel_jobs] [conversion_extension] [conversion_directory] [meshlab_filters] [meshlab_filters_options] [final_extension] 12 | convert_meshes_to_pcd.sh '/path/to/folder' .ply 13 | ``` 14 | 15 | 16 | ## convert mesh to pcd using pcl vtk_io 17 | ``` 18 | rosrun dynamic_robot_localization drl_mesh_to_pcd [path/]input.[pcd|obj|ply|stl|vtk] [path/]output.pcd [-binary 0|1] [-compressed 0|1] [-type PointNormal|PointXYZRGBNormal|auto] 19 | rosrun dynamic_robot_localization drl_mesh_to_pcd input.ply output.pcd -binary 1 -compressed 1 -type auto 20 | ``` 21 | 22 | 23 | ## convert ply to pcd using pcl utils 24 | ``` 25 | pcl_ply2pcd -format 1 input.ply output.pcd 26 | ``` 27 | 28 | 29 | ## convert mesh to pcd using OpenSceneGraph 30 | ``` 31 | rosrun mesh_to_pointcloud mesh_to_pcd [path/]input.[3dc|3ds|asc|ac|bsp|dae|dw|dxf|fbx|flt|gem|geo|iv|ive|logo|lwo|lw|lws|md2|obj|ogr|osg|pfb|ply|shp|stl|x|wrl] [path/]output.pcd [-binary 0|1] [-compressed 0|1] [-type PointXYZ|PointNormal|PointXYZRGB|PointXYZRGBNormal] 32 | rosrun mesh_to_pointcloud mesh_to_pcd input.ply input.pcd -binary 1 -type PointXYZRGBNormal 33 | ``` 34 | 35 | 36 | ## convert files in meshlab 37 | ``` 38 | meshlabserver -i input.stl -o output.ply -s filters.mlx -om vc vq vn 39 | ``` 40 | 41 | 42 | ## change line in file 43 | ``` 44 | sed -i '{line_number} c\{text}' {file_name} 45 | sed -i '11 c\property float curvature' input.ply 46 | ``` 47 | 48 | 49 | ## replace first occurrence of text 50 | ``` 51 | sed -i 's/find/replace/' input_file 52 | sed -i 's/quality/curvature/' input.ply 53 | ``` 54 | 55 | 56 | # view point clouds from .pcd files 57 | ``` 58 | pcl_viewer input.pcd -normals 1 -normals_scale 0.01 -pc 1 -pc_scale 1 -ax 1 -opaque 0.1 59 | ``` 60 | -------------------------------------------------------------------------------- /install/e_build.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | catkin_ws_path=${1:-"$HOME/catkin_ws_drl"} 4 | number_of_cpu_threads_for_compilation=${2:-2} 5 | use_catkin_tools=${3:-true} 6 | ros_version=${4:-"$(rosversion -d)"} 7 | catkin_ws_path_to_extend=${5:-"/opt/ros/$ros_version"} 8 | 9 | 10 | echo -e "\n\n" 11 | echo "####################################################################################################" 12 | echo "##### Building catkin workspace" 13 | echo "####################################################################################################" 14 | 15 | cd "${catkin_ws_path}" 16 | source "${catkin_ws_path_to_extend}/setup.bash" 17 | 18 | if [ $use_catkin_tools = true ]; then 19 | devel_file="${catkin_ws_path}/devel_release/setup.bash" 20 | ws_overlay_url="https://catkin-tools.readthedocs.io/en/latest/mechanics.html#workspace-chaining-extending" 21 | catkin build pcl -s -j${number_of_cpu_threads_for_compilation} 22 | source ${devel_file} 23 | catkin build -s -j${number_of_cpu_threads_for_compilation} 24 | else 25 | ws_overlay_url="http://wiki.ros.org/catkin/Tutorials/workspace_overlaying" 26 | devel_file="${catkin_ws_path}/devel_isolated/setup.bash" 27 | catkin_make_isolated --pkg pcl --force-cmake -DCMAKE_BUILD_TYPE=Release -j${number_of_cpu_threads_for_compilation} 28 | source ${devel_file} 29 | catkin_make_isolated -DCMAKE_BUILD_TYPE=Release -j${number_of_cpu_threads_for_compilation} 30 | fi 31 | 32 | source ${devel_file} 33 | rospack profile 34 | echo -e "\n\nsource ${devel_file}\n\n" >> ~/.bashrc 35 | 36 | echo -e "\n\n" 37 | echo "----------------------------------------------------------------------------------------------------" 38 | echo "===== Finished building the catkin workspace" 39 | echo "----- For using this workspace source the file ${devel_file}" 40 | echo "----- For overlaying / extending this workspace please read the following documentation:" 41 | echo "----- ${ws_overlay_url}" 42 | echo "----- Thank you for using the dynamic_robot_localization package!" 43 | echo "----- Suggestions for improvements and pull requests are always welcomed!" 44 | echo "----------------------------------------------------------------------------------------------------" 45 | -------------------------------------------------------------------------------- /include/dynamic_robot_localization/common/performance_timer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 5 | #ifdef _WIN32 // Windows system specific 6 | #include 7 | #else // Unix based system specific 8 | #include 9 | #endif 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | #include 17 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 18 | 19 | 20 | namespace dynamic_robot_localization { 21 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | class PerformanceTimer { 23 | public: 24 | PerformanceTimer(); 25 | virtual ~PerformanceTimer(); 26 | 27 | void start(); // start timer 28 | void stop(); // stop the timer 29 | void reset(); 30 | void restart(); 31 | double getElapsedTimeInSec(); // get elapsed time in second 32 | double getElapsedTimeInMilliSec(); // get elapsed time in milli-second 33 | double getElapsedTimeInMicroSec(); // get elapsed time in micro-second 34 | std::string getElapsedTimeFormated(); 35 | 36 | 37 | private: 38 | double elapsedTimeMicroSec; // starting time in micro-second 39 | bool stopped; // stop flag 40 | 41 | #ifdef _WIN32 42 | LARGE_INTEGER frequencyWin; // ticks per second 43 | LARGE_INTEGER startCountWin; 44 | LARGE_INTEGER endCountWin; 45 | #else 46 | timeval startCount; 47 | timeval endCount; 48 | #endif 49 | 50 | void calculateElapsedTimeMicroSec(); 51 | }; 52 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 53 | } /* namespace dynamic_robot_localization */ 54 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dynamic_robot_localization 4 | 1.2.0 5 | The dynamic_robot_localization package offers 3 DoF and 6 DoF robot self-localization along with a set of libraries suitable for generic point cloud alignment 6 | Carlos Miguel Correia da Costa 7 | BSD 8 | Carlos Miguel Correia da Costa 9 | 10 | 11 | catkin 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | laserscan_to_pointcloud 20 | pose_to_tf_publisher 21 | 22 | 23 | angles 24 | dynamic_reconfigure 25 | geometry_msgs 26 | message_generation 27 | pcl_conversions 28 | roscpp 29 | rosconsole 30 | rostime 31 | sensor_msgs 32 | std_msgs 33 | tf2 34 | tf2_ros 35 | xmlrpcpp 36 | 37 | 38 | eigen 39 | pcl 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | message_runtime 48 | map_server 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/cluttered_environments_dynamic_large_map_slam_2d.yaml: -------------------------------------------------------------------------------- 1 | tracking_matchers: 2 | ignore_height_corrections: false 3 | point_matchers: 4 | # iterative_closest_point: 5 | iterative_closest_point_2d: 6 | # iterative_closest_point_non_linear: 7 | # iterative_closest_point_generalized: 8 | # normal_distributions_transform_2d: 9 | convergence_time_limit_seconds: 0.09 10 | max_correspondence_distance: 0.5 11 | transformation_epsilon: 0.000001 12 | euclidean_fitness_epsilon: 0.000001 13 | max_number_of_registration_iterations: 150 14 | max_number_of_ransac_iterations: 20 15 | ransac_outlier_rejection_threshold: 0.035 16 | match_only_keypoints: false 17 | display_cloud_aligment: false 18 | maximum_number_of_displayed_correspondences: 0 19 | # gicp 20 | rotation_epsilon: 0.002 21 | correspondence_randomness: 50 22 | maximum_optimizer_iterations: 100 23 | use_reciprocal_correspondences: false 24 | # ndt_2d 25 | grid_center_x: 10.0 26 | grid_center_y: 5.0 27 | grid_step_x: 0.05 28 | grid_step_y: 0.05 29 | grid_extent_x: 20.0 30 | grid_extent_y: 20.0 31 | grid_optimization_step_size_x: 0.05 32 | grid_optimization_step_size_y: 0.05 33 | grid_optimization_step_size_theta: 0.025 34 | 35 | 36 | outlier_detectors: 37 | euclidean_outlier_detector: 38 | max_inliers_distance: 0.07 39 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 40 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 41 | 42 | 43 | #cloud_analyzers: 44 | # compute_inliers_angular_distribution: true 45 | # compute_outliers_angular_distribution: true 46 | # angular_distribution_analyzer: 47 | # number_of_angular_bins: 180 48 | 49 | 50 | transformation_validators: 51 | euclidean_transformation_validator: 52 | max_transformation_angle: 0.7 53 | max_transformation_distance: 0.5 54 | max_new_pose_diff_angle: 1.1 55 | max_new_pose_diff_distance: 1.0 56 | max_root_mean_square_error: 0.04 57 | max_outliers_percentage: 0.6 58 | min_inliers_angular_distribution: 0.125 59 | max_outliers_angular_distribution: 0.875 60 | -------------------------------------------------------------------------------- /install/b_workspace.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | echo "####################################################################################################" 4 | echo "##### Initializing catkin workspace..." 5 | echo "####################################################################################################" 6 | 7 | catkin_ws_path=${1:-"$HOME/catkin_ws_drl"} 8 | use_catkin_tools=${2:-true} 9 | ros_version=${3:-"$(rosversion -d)"} 10 | catkin_ws_path_to_extend=${4:-"/opt/ros/$ros_version"} 11 | 12 | 13 | function AddProfile { 14 | catkin config --profile $1 -x _$1 --cmake-args -DCMAKE_BUILD_TYPE=$2 15 | catkin profile set $1 16 | catkin config --extend $3 17 | catkin config --append-args --cmake-args -DCMAKE_CXX_FLAGS="-Wall -Wextra" 18 | } 19 | 20 | source "${catkin_ws_path_to_extend}/setup.bash" 21 | 22 | ls "${catkin_ws_path}/src" &> /dev/null 23 | if [ $? -ne 0 ]; then 24 | mkdir -p "${catkin_ws_path}/src" 25 | fi 26 | 27 | ws_initialized=false 28 | if [ $use_catkin_tools = true ]; then 29 | ls "${catkin_ws_path}/.catkin_tools" &> /dev/null 30 | if [ $? -ne 0 ]; then 31 | echo "===========================================" 32 | echo "==> Initializing catkin tools workspace..." 33 | cd "${catkin_ws_path}" 34 | catkin config --init --no-install --extend ${catkin_ws_path_to_extend} 35 | 36 | echo "===========================================" 37 | echo "==> Creating Debug profile..." 38 | AddProfile debug Debug ${catkin_ws_path_to_extend} 39 | 40 | echo "===========================================" 41 | echo "==> Creating RelWithDebInfo profile..." 42 | AddProfile release_with_debug_info RelWithDebInfo ${catkin_ws_path_to_extend} 43 | 44 | echo "===========================================" 45 | echo "==> Creating Release profile..." 46 | AddProfile release Release ${catkin_ws_path_to_extend} 47 | ws_initialized=true 48 | fi 49 | else 50 | ls "${catkin_ws_path}/src/CMakeLists.txt" &> /dev/null 51 | if [ $? -ne 0 ]; then 52 | echo "==> Initializing catkin workspace..." 53 | cd "${catkin_ws_path}/src" 54 | catkin_init_workspace 55 | ws_initialized=true 56 | fi 57 | fi 58 | 59 | if [ $ws_initialized = true ]; then 60 | echo "--------------------------------------------------------" 61 | echo "==> Finished workspace initialization" 62 | echo "--------------------------------------------------------" 63 | else 64 | echo "--------------------------------------------------------" 65 | echo "==> Workspace already exists. Skipping initialization..." 66 | echo "--------------------------------------------------------" 67 | fi 68 | -------------------------------------------------------------------------------- /include/dynamic_robot_localization/common/cumulative_static_transform_broadcaster.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 5 | // std includes 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // ROS includes 14 | #include 15 | #include 16 | 17 | // external libs includes 18 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 19 | 20 | 21 | namespace dynamic_robot_localization { 22 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 23 | class CumulativeStaticTransformBroadcaster { 24 | public: 25 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 26 | CumulativeStaticTransformBroadcaster() {} 27 | virtual ~CumulativeStaticTransformBroadcaster() {} 28 | static std::shared_ptr< CumulativeStaticTransformBroadcaster > getSingleton(ros::NodeHandlePtr& node_handle); 29 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 30 | 31 | void setup(ros::NodeHandlePtr& node_handle); 32 | void updateTFCache(const tf2_msgs::TFMessageConstPtr& tf_msg); 33 | void updateTFCache(const std::vector& tfs); 34 | void sendTransform(const geometry_msgs::TransformStamped& tf); 35 | void sendTransform(const std::vector& tfs); 36 | void sendCachedTFs(); 37 | 38 | private: 39 | ros::Subscriber static_tf_subscriber_; 40 | ros::Publisher static_tf_publisher_; 41 | std::map< std::string, geometry_msgs::TransformStamped > cached_static_tfs_; 42 | std::shared_ptr< std::mutex > cached_static_tfs_mutex_; 43 | static std::shared_ptr< std::mutex > singleton__mutex_; 44 | static std::shared_ptr< CumulativeStaticTransformBroadcaster > singleton_; 45 | }; 46 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 47 | } /* namespace dynamic_robot_localization */ 48 | -------------------------------------------------------------------------------- /include/dynamic_robot_localization/common/pointcloud_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /**\file pointcloud_utils.h 4 | * \brief Description... 5 | * 6 | * @version 1.0 7 | * @author Carlos Miguel Correia da Costa 8 | */ 9 | 10 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 11 | // std includes 12 | #include 13 | #include 14 | 15 | // PCL includes 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | 23 | namespace dynamic_robot_localization { 24 | // ############################################################################ pointcloud_utils ########################################################################### 25 | 26 | namespace pointcloud_utils { 27 | 28 | template 29 | void concatenatePointClouds(const std::vector< typename pcl::PointCloud::Ptr >& pointclouds, typename pcl::PointCloud::Ptr& pointcloud_out); 30 | 31 | template 32 | void normalizePointCloudNormals(pcl::PointCloud& pointcloud); 33 | 34 | template 35 | void colorizePointCloudWithCurvature(pcl::PointCloud& pointcloud); 36 | 37 | template 38 | void removePointsOnSensorOrigin(pcl::PointCloud& pointcloud); 39 | 40 | template 41 | void colorizePointCloudClusters(const pcl::PointCloud& pointcloud, const std::vector& cluster_indices, pcl::PointCloud& pointcloud_colored_out); 42 | 43 | template 44 | void extractPointCloudClusters(const pcl::PointCloud& pointcloud, const std::vector& cluster_indices, const std::vector& selected_clusters, pcl::PointCloud& pointcloud_out); 45 | 46 | template 47 | float distanceSquaredToOrigin(const PointT& point); 48 | 49 | std::string getFileExtension(const std::string& filename); 50 | 51 | std::string parseFilePath(const std::string& filename, const std::string& folder); 52 | 53 | } /* namespace pointcloud_utils */ 54 | } /* namespace dynamic_robot_localization */ 55 | 56 | 57 | #ifdef DRL_NO_PRECOMPILE 58 | #include 59 | #endif 60 | 61 | -------------------------------------------------------------------------------- /yaml/configs/filters/filters_large_map_2d.yaml: -------------------------------------------------------------------------------- 1 | localization_point_type: 'PointXYZINormal' 2 | 3 | filters: 4 | reference_pointcloud: 5 | 1_voxel_grid: 6 | leaf_size_x: 0.02 7 | leaf_size_y: 0.02 8 | leaf_size_z: 0.02 9 | filter_limit_field_name: 'z' 10 | filter_limit_min: -0.001 11 | filter_limit_max: 1.8 12 | filtered_cloud_publish_topic: '' 13 | downsample_all_data: false 14 | save_leaf_layout: false 15 | ambient_pointcloud: 16 | 1_voxel_grid: 17 | leaf_size_x: 0.02 18 | leaf_size_y: 0.02 19 | leaf_size_z: 0.02 20 | filter_limit_field_name: 'z' 21 | filter_limit_min: -1.0 22 | filter_limit_max: 2.0 23 | filtered_cloud_publish_topic: '' 24 | downsample_all_data: false 25 | save_leaf_layout: false 26 | # 2_approximate_voxel_grid: 27 | # leaf_size_x: 0.025 28 | # leaf_size_y: 0.025 29 | # leaf_size_z: 0.025 30 | # filtered_cloud_publish_topic: '' 31 | # downsample_all_data: false 32 | # 3_radius_outlier_removal: 33 | # radius_search: 2.0 34 | # min_neighbors_in_radius: 6 35 | # invert_removal: false 36 | # filtered_cloud_publish_topic: '' 37 | # 4_crop_box: 38 | # box_min_x: -20.0 39 | # box_min_y: -20.0 40 | # box_min_z: -20.0 41 | # box_max_x: 20.0 42 | # box_max_y: 20.0 43 | # box_max_z: 20.0 44 | # box_translation_x: 0.0 45 | # box_translation_y: 0.0 46 | # box_translation_z: 0.0 47 | # box_rotation_roll: 0.0 48 | # box_rotation_pitch: 0.0 49 | # box_rotation_yaw: 0.0 50 | # invert_selection: false 51 | # filtered_cloud_publish_topic: '' 52 | 5_random_sample: 53 | number_of_random_samples: 300 54 | invert_sampling: false 55 | filtered_cloud_publish_topic: 'ambient_pointcloud_filtered' 56 | # 6_statistical_outlier_removal: 57 | # number_of_neighbors_for_mean_distance_estimation: 6 58 | # standard_deviation_multiplier: 2.0 59 | # invert_selection: false 60 | # filtered_cloud_publish_topic: '' 61 | # 7_covariance_sampling: 62 | # number_of_samples: 100 63 | # filtered_cloud_publish_topic: '' 64 | # 8_radius_outlier_removal: 65 | # radius_search: 1.0 66 | # min_neighbors_in_radius: 2 67 | # invert_removal: false 68 | # filtered_cloud_publish_topic: '' 69 | -------------------------------------------------------------------------------- /yaml/configs/filters/filters_large_map_2d_slam.yaml: -------------------------------------------------------------------------------- 1 | localization_point_type: 'PointXYZINormal' 2 | 3 | filters: 4 | reference_pointcloud: 5 | 1_voxel_grid: 6 | leaf_size_x: 0.02 7 | leaf_size_y: 0.02 8 | leaf_size_z: 0.02 9 | filter_limit_field_name: 'z' 10 | filter_limit_min: -0.001 11 | filter_limit_max: 1.8 12 | filtered_cloud_publish_topic: '' 13 | downsample_all_data: false 14 | save_leaf_layout: false 15 | ambient_pointcloud: 16 | 1_voxel_grid: 17 | leaf_size_x: 0.02 18 | leaf_size_y: 0.02 19 | leaf_size_z: 0.02 20 | filter_limit_field_name: 'z' 21 | filter_limit_min: -1.0 22 | filter_limit_max: 2.0 23 | filtered_cloud_publish_topic: '' 24 | downsample_all_data: false 25 | save_leaf_layout: false 26 | # 2_approximate_voxel_grid: 27 | # leaf_size_x: 0.025 28 | # leaf_size_y: 0.025 29 | # leaf_size_z: 0.025 30 | # filtered_cloud_publish_topic: '' 31 | # downsample_all_data: false 32 | # 3_radius_outlier_removal: 33 | # radius_search: 2.0 34 | # min_neighbors_in_radius: 6 35 | # invert_removal: false 36 | # filtered_cloud_publish_topic: '' 37 | # 4_crop_box: 38 | # box_min_x: -20.0 39 | # box_min_y: -20.0 40 | # box_min_z: -20.0 41 | # box_max_x: 20.0 42 | # box_max_y: 20.0 43 | # box_max_z: 20.0 44 | # box_translation_x: 0.0 45 | # box_translation_y: 0.0 46 | # box_translation_z: 0.0 47 | # box_rotation_roll: 0.0 48 | # box_rotation_pitch: 0.0 49 | # box_rotation_yaw: 0.0 50 | # invert_selection: false 51 | # filtered_cloud_publish_topic: '' 52 | 5_random_sample: 53 | number_of_random_samples: 1000 54 | invert_sampling: false 55 | filtered_cloud_publish_topic: 'ambient_pointcloud_filtered' 56 | # 6_statistical_outlier_removal: 57 | # number_of_neighbors_for_mean_distance_estimation: 6 58 | # standard_deviation_multiplier: 2.0 59 | # invert_selection: false 60 | # filtered_cloud_publish_topic: '' 61 | # 7_covariance_sampling: 62 | # number_of_samples: 100 63 | # filtered_cloud_publish_topic: '' 64 | # 8_radius_outlier_removal: 65 | # radius_search: 1.0 66 | # min_neighbors_in_radius: 2 67 | # invert_removal: false 68 | # filtered_cloud_publish_topic: '' 69 | -------------------------------------------------------------------------------- /src/common/performance_timer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace dynamic_robot_localization { 4 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 5 | PerformanceTimer::PerformanceTimer() { 6 | reset(); 7 | } 8 | 9 | 10 | PerformanceTimer::~PerformanceTimer() {} 11 | 12 | 13 | void PerformanceTimer::start() { 14 | stopped = 0; 15 | #ifdef _WIN32 16 | QueryPerformanceCounter(&startCountWin); 17 | #else 18 | gettimeofday(&startCount, NULL); 19 | #endif 20 | } 21 | 22 | 23 | void PerformanceTimer::stop() { 24 | stopped = true; 25 | 26 | #ifdef _WIN32 27 | QueryPerformanceCounter(&endCountWin); 28 | #else 29 | gettimeofday(&endCount, NULL); 30 | #endif 31 | 32 | calculateElapsedTimeMicroSec(); 33 | } 34 | 35 | 36 | void PerformanceTimer::reset() { 37 | #ifdef _WIN32 38 | QueryPerformanceFrequency(&frequencyWin); 39 | startCountWin.QuadPart = 0; 40 | endCountWin.QuadPart = 0; 41 | #else 42 | startCount.tv_sec = startCount.tv_usec = 0; 43 | endCount.tv_sec = endCount.tv_usec = 0; 44 | #endif 45 | 46 | stopped = false; 47 | elapsedTimeMicroSec = 0; 48 | } 49 | 50 | void PerformanceTimer::restart() { 51 | reset(); 52 | start(); 53 | } 54 | 55 | 56 | double PerformanceTimer::getElapsedTimeInSec() { 57 | return getElapsedTimeInMicroSec() / 1000000.0; 58 | } 59 | 60 | 61 | double PerformanceTimer::getElapsedTimeInMilliSec() { 62 | 63 | return getElapsedTimeInMicroSec() / 1000.0; 64 | } 65 | 66 | 67 | double PerformanceTimer::getElapsedTimeInMicroSec() { 68 | if (!stopped) { 69 | #ifdef _WIN32 70 | QueryPerformanceCounter(&endCountWin); 71 | #else 72 | gettimeofday(&endCount, NULL); 73 | #endif 74 | calculateElapsedTimeMicroSec(); 75 | } 76 | 77 | return elapsedTimeMicroSec; 78 | } 79 | 80 | 81 | void PerformanceTimer::calculateElapsedTimeMicroSec() { 82 | #ifdef _WIN32 83 | elapsedTimeMicroSec = (endCountWin.QuadPart - startCountWin.QuadPart) * 1000000.0 / frequencyWin.QuadPart; 84 | #else 85 | elapsedTimeMicroSec = (double)((endCount.tv_sec - startCount.tv_sec) * 1000000 + (endCount.tv_usec - startCount.tv_usec)); 86 | #endif 87 | } 88 | 89 | 90 | std::string PerformanceTimer::getElapsedTimeFormated() { 91 | return dynamic_robot_localization::TimeUtils::formatSecondsToDate(getElapsedTimeInSec()); 92 | } 93 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 94 | } /* namespace dynamic_robot_localization */ 95 | 96 | -------------------------------------------------------------------------------- /include/dynamic_robot_localization/common/impl/math_utils.hpp: -------------------------------------------------------------------------------- 1 | /**\file math_utils.hpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 11 | 12 | namespace dynamic_robot_localization { 13 | namespace math_utils { 14 | 15 | 16 | template 17 | bool isTransformValid(const Eigen::Matrix& transform) { 18 | if (!std::isfinite(transform(0, 0)) || !std::isfinite(transform(0, 1)) || !std::isfinite(transform(0, 2)) || !std::isfinite(transform(0, 3)) || 19 | !std::isfinite(transform(1, 0)) || !std::isfinite(transform(1, 1)) || !std::isfinite(transform(1, 2)) || !std::isfinite(transform(1, 3)) || 20 | !std::isfinite(transform(2, 0)) || !std::isfinite(transform(2, 1)) || !std::isfinite(transform(2, 2)) || !std::isfinite(transform(2, 3)) || 21 | !std::isfinite(transform(3, 0)) || !std::isfinite(transform(3, 1)) || !std::isfinite(transform(3, 2)) || !std::isfinite(transform(3, 3))) { 22 | return false; 23 | } 24 | return true; 25 | } 26 | 27 | 28 | template 29 | std::string convertTransformToString(const Eigen::Matrix& transform, const std::string& line_prefix, const std::string& line_suffix, const std::string& number_separator) { 30 | std::stringstream ss; 31 | ss << line_prefix << std::setw(18) << transform(0, 0) << number_separator << std::setw(18) << transform(0, 1) << number_separator << std::setw(18) << transform(0, 2) << number_separator << std::setw(18) << transform(0, 3) << line_suffix 32 | << line_prefix << std::setw(18) << transform(1, 0) << number_separator << std::setw(18) << transform(1, 1) << number_separator << std::setw(18) << transform(1, 2) << number_separator << std::setw(18) << transform(1, 3) << line_suffix 33 | << line_prefix << std::setw(18) << transform(2, 0) << number_separator << std::setw(18) << transform(2, 1) << number_separator << std::setw(18) << transform(2, 2) << number_separator << std::setw(18) << transform(2, 3) << line_suffix 34 | << line_prefix << std::setw(18) << transform(3, 0) << number_separator << std::setw(18) << transform(3, 1) << number_separator << std::setw(18) << transform(3, 2) << number_separator << std::setw(18) << transform(3, 3) << line_suffix; 35 | return ss.str(); 36 | } 37 | 38 | 39 | } /* namespace math_utils */ 40 | } /* namespace dynamic_robot_localization */ 41 | -------------------------------------------------------------------------------- /install/c_repositories.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | echo "####################################################################################################" 4 | echo "##### Cloning git repositories..." 5 | echo "####################################################################################################" 6 | 7 | catkin_ws_path=${1:-"$HOME/catkin_ws_drl"} 8 | repositories=${2:-$(cat <<-END 9 | https://github.com/carlosmccosta|dynamic_robot_localization 10 | https://github.com/carlosmccosta|pose_to_tf_publisher 11 | https://github.com/carlosmccosta|laserscan_to_pointcloud 12 | https://github.com/carlosmccosta|mesh_to_pointcloud 13 | https://github.com/carlosmccosta|pcl 14 | https://github.com/ros-perception|pcl_msgs 15 | https://github.com/ros-perception|perception_pcl 16 | END 17 | ) 18 | } 19 | 20 | 21 | cd "${catkin_ws_path}/src/" &> /dev/null 22 | if [ $? -ne 0 ]; then 23 | mkdir -p "${catkin_ws_path}/src" 24 | cd "${catkin_ws_path}/src/" 25 | fi 26 | 27 | 28 | ls "${catkin_ws_path}/src/.rosinstall" &> /dev/null 29 | if [ $? -ne 0 ]; then 30 | wstool init 31 | fi 32 | 33 | for git_repository in ${repositories} 34 | do 35 | IFS="|"; set -- ${git_repository}; 36 | git_repository_url="${1}/${2}.git" 37 | ls "${2}" &> /dev/null 38 | if [ $? -ne 0 ]; then 39 | echo -e "\n\n" 40 | echo "-------------------------------------------" 41 | echo "==> Cloning ${2} from ${git_repository_url}" 42 | git clone ${git_repository_url} 43 | else 44 | echo -e "\n\n" 45 | echo "-------------------------------------------" 46 | echo "==> Updating ${2}" 47 | cd ${2} 48 | git pull 49 | cd .. 50 | fi 51 | wstool info "${2}" &> /dev/null 52 | if [ $? -ne 0 ]; then 53 | wstool set "${2}" "${git_repository_url}" --git -y 54 | fi 55 | done 56 | 57 | 58 | cd "${catkin_ws_path}/src/pcl_msgs" 59 | git checkout noetic-devel 60 | 61 | cd "${catkin_ws_path}/src/perception_pcl" 62 | git checkout melodic-devel 63 | 64 | 65 | cd "${catkin_ws_path}" 66 | find ./src -name "*.bash" -exec chmod +x {} \; 67 | find ./src -name "*.cfg" -exec chmod +x {} \; 68 | find ./src -name "*.sh" -exec chmod +x {} \; 69 | 70 | 71 | echo -e "\n\n" 72 | echo "----------------------------------------------------------------------------------------------------" 73 | echo ">>>>> Cloning git repositories finished" 74 | echo ">>>>> For updating each git repository use: git pull" 75 | echo ">>>>> For updating all repositories use:" 76 | echo ">>>>> ${catkin_ws_path}/src/dynamic_robot_localization/install/repositories_update.sh" 77 | echo ">>>>> or" 78 | echo ">>>>> cd ${catkin_ws_path}/src" 79 | echo ">>>>> wstool status" 80 | echo ">>>>> Commit or stash modified files" 81 | echo ">>>>> wstool update" 82 | echo "----------------------------------------------------------------------------------------------------" 83 | -------------------------------------------------------------------------------- /yaml/configs/pose_tracking/static_3d_tof.yaml: -------------------------------------------------------------------------------- 1 | #message_management: 2 | # maximum_number_points_ambient_pointcloud_circular_buffer: 750 3 | 4 | normal_estimators: 5 | reference_pointcloud: 6 | display_normals: false 7 | display_occupancy_grid_pointcloud: false 8 | use_filtered_cloud_as_normal_estimation_surface: false 9 | flip_normals_using_occupancy_grid_analysis: true 10 | occupancy_grid_analysis_radius_resolution_percentage: 4.0 11 | # normal_estimation_omp: 12 | # search_k: 0 13 | # search_radius: 0.35 14 | ambient_pointcloud: 15 | compute_normals_when_tracking_pose: false 16 | compute_normals_when_recovering_pose_tracking: false 17 | compute_normals_when_estimating_initial_pose: true 18 | use_filtered_cloud_as_normal_estimation_surface: false 19 | display_normals: false 20 | normal_estimation_omp: 21 | search_k: 0 22 | search_radius: 0.35 23 | 24 | 25 | 26 | tracking_matchers: 27 | ignore_height_corrections: false 28 | point_matchers: 29 | iterative_closest_point: 30 | # iterative_closest_point_non_linear: 31 | # iterative_closest_point_with_normals: 32 | # iterative_closest_point_generalized: 33 | convergence_time_limit_seconds: 0.250 34 | max_correspondence_distance: 2.0 35 | transformation_epsilon: 0.0001 36 | euclidean_fitness_epsilon: 0.0001 37 | max_number_of_registration_iterations: 150 38 | max_number_of_ransac_iterations: 50 39 | ransac_outlier_rejection_threshold: 0.05 40 | match_only_keypoints: false 41 | display_cloud_aligment: false 42 | maximum_number_of_displayed_correspondences: 0 43 | rotation_epsilon: 0.002 44 | correspondence_randomness: 50 45 | maximum_optimizer_iterations: 100 46 | use_reciprocal_correspondences: false 47 | 48 | 49 | outlier_detectors: 50 | euclidean_outlier_detector: 51 | max_inliers_distance: 0.06 52 | aligned_pointcloud_outliers_publish_topic: 'aligned_pointcloud_outliers' 53 | aligned_pointcloud_inliers_publish_topic: 'aligned_pointcloud_inliers' 54 | 55 | 56 | transformation_validators: 57 | euclidean_transformation_validator: 58 | max_transformation_angle: 0.3 59 | max_transformation_distance: 0.05 60 | max_new_pose_diff_angle: 1.0 61 | max_new_pose_diff_distance: 0.5 62 | max_root_mean_square_error: 0.05 63 | max_outliers_percentage: 0.07 64 | 65 | 66 | transformation_validators_tracking_recovery: 67 | euclidean_transformation_validator: 68 | max_root_mean_square_error: 0.05 69 | max_outliers_percentage: 0.1 70 | -------------------------------------------------------------------------------- /yaml/configs/filters/filters_3d_tof.yaml: -------------------------------------------------------------------------------- 1 | localization_point_type: 'PointXYZRGBNormal' 2 | 3 | filters: 4 | reference_pointcloud: 5 | 1_voxel_grid: 6 | leaf_size_x: 0.02 7 | leaf_size_y: 0.02 8 | leaf_size_z: 0.02 9 | filter_limit_field_name: 'z' 10 | filter_limit_min: -5.0 11 | filter_limit_max: 5.0 12 | filtered_cloud_publish_topic: '' 13 | downsample_all_data: true 14 | save_leaf_layout: false 15 | # 2_covariance_sampling: 16 | # number_of_samples: 10000 17 | # filtered_cloud_publish_topic: '' 18 | # 3_random_sample: 19 | # number_of_random_samples: 6000 20 | # invert_sampling: false 21 | # filtered_cloud_publish_topic: '' 22 | ambient_pointcloud: 23 | 1_voxel_grid: 24 | leaf_size_x: 0.02 25 | leaf_size_y: 0.02 26 | leaf_size_z: 0.02 27 | filter_limit_field_name: 'z' 28 | filter_limit_min: -5.0 29 | filter_limit_max: 5.0 30 | filtered_cloud_publish_topic: '' 31 | downsample_all_data: true 32 | save_leaf_layout: false 33 | # 2_approximate_voxel_grid: 34 | # leaf_size_x: 0.025 35 | # leaf_size_y: 0.025 36 | # leaf_size_z: 0.025 37 | # filtered_cloud_publish_topic: '' 38 | # downsample_all_data: false 39 | # 3_radius_outlier_removal: 40 | # radius_search: 2.0 41 | # min_neighbors_in_radius: 6 42 | # invert_removal: false 43 | # filtered_cloud_publish_topic: '' 44 | 4_crop_box: 45 | box_min_x: -1.5 46 | box_min_y: -1.5 47 | box_min_z: 0.5 48 | box_max_x: 1.5 49 | box_max_y: 1.5 50 | box_max_z: 2.25 51 | box_translation_x: 0.0 52 | box_translation_y: 0.0 53 | box_translation_z: 0.0 54 | box_rotation_roll: 0.0 55 | box_rotation_pitch: 0.0 56 | box_rotation_yaw: 0.0 57 | invert_selection: false 58 | filtered_cloud_publish_topic: '' 59 | 5_random_sample: 60 | number_of_random_samples: 425 61 | invert_sampling: false 62 | filtered_cloud_publish_topic: 'ambient_pointcloud_filtered' 63 | # 6_statistical_outlier_removal: 64 | # number_of_neighbors_for_mean_distance_estimation: 6 65 | # standard_deviation_multiplier: 2.0 66 | # invert_selection: false 67 | # filtered_cloud_publish_topic: '' 68 | # 7_covariance_sampling: 69 | # number_of_samples: 1000 70 | # filtered_cloud_publish_topic: '' 71 | # 8_radius_outlier_removal: 72 | # radius_search: 1.0 73 | # min_neighbors_in_radius: 2 74 | # invert_removal: false 75 | # filtered_cloud_publish_topic: '' 76 | -------------------------------------------------------------------------------- /docs/mesh_tessellation.md: -------------------------------------------------------------------------------- 1 | # Generation of a point cloud with uniform density using [MeshLab](http://www.meshlab.net): 2 | 3 | 1. __Scale the point cloud to the correct units (meters)__ 4 | - Render -> Show box corners 5 | - To see the range in XYZ of the mesh (to confirm if it is in mm, cm, m, ...) 6 | - Filters -> Normals, curvatures and Orientation -> Transform: Scale, Normalize 7 | - To make uniform scaling (0.001 in XYZ to change from mm to m) 8 | 9 | 2. __Preprocess the mesh to ensure that the meshlab algorithms work correctly:__ 10 | - Filters -> Cleaning and Repairing -> Remove Faces from Non Manifold Edges 11 | - Filters -> Normals, curvatures and Orientation -> Normalize Vertex Normals 12 | 13 | 3. __Increase the density of triangles using either:__ 14 | 1. Mid point Subdivision and subsampling 15 | * Achieves better accuracy since the generated mesh triangles are on the same surface of their original triangles (uses subdivision) 16 | * Mid point division usually does not achieve uniform point density, requiring a subsampling algorithm that loses mesh information and generates a point cloud 17 | - Mid point Subdivision 18 | - Filters -> Remeshing, Simplification and Reconstruction -> Subdivision Surfaces: Midpoint 19 | - Iterations: 100 20 | - Edge Threshold (world unit): 0.002 m (for example, but can be + or - depending on the size of the part and the desired accuracy) 21 | - Subsampling of the mesh for having +- an uniform density of points using either: 22 | 1. Poisson-disk Sampling (preferred method since it creates points in the middle of the triangles using a Poison distribution) 23 | - Filters -> Sampling -> Poisson-disk Sampling 24 | 2. Filters -> Sampling -> Clustered Vertex Subsampling 25 | 3. Filters -> Point Set -> Point Cloud Simplification 26 | 2. Uniform Mesh Resampling 27 | * Achieves better point density uniformity and keeps the mesh information 28 | * May generate artifacts which cause the generated mesh surface to diverge slightly from the original mesh 29 | - Filters -> Remeshing, Simplification and Reconstruction -> Uniform Mesh Resampling 30 | - Precision (world unit): 0.002 m (for example, but can be + or - depending on the size of the part and the desired accuracy) 31 | - Clean vertices 32 | 33 | 3. __Post processing__ 34 | - View -> Show Layer Dialog 35 | - Hide the original mesh 36 | - Select the view of the mesh and then the points to confirm that the generate mesh / point cloud is adequate 37 | - Render -> Show Normal -> For confirming that there are normals for the point cloud and they are correct 38 | - Filters -> Normals, curvatures and Orientation -> Normalize Vertex Normals 39 | 40 | 4. __Export__ 41 | - File -> Export Mesh As 42 | - .ply 43 | - Save Vert Normal 44 | - Binary encoding 45 | -------------------------------------------------------------------------------- /include/dynamic_robot_localization/registration_covariance_estimators/impl/registration_covariance_point_to_point_pm_3d.hpp: -------------------------------------------------------------------------------- 1 | /**\file registration_covariance_point_to_point_pm_3d.hpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 11 | 12 | namespace dynamic_robot_localization { 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 15 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 16 | 17 | // ============================================================================= ============================================================================ 18 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 19 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 20 | 21 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 23 | // ============================================================================= =========================================================================== 24 | 25 | // ============================================================================= ======================================================================= 26 | // ============================================================================= ======================================================================= 27 | 28 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 30 | 31 | } /* namespace dynamic_robot_localization */ 32 | -------------------------------------------------------------------------------- /src/common/registration_visualizer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2011, Willow Garage, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of Willow Garage, Inc. nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * $Id$ 37 | * 38 | */ 39 | 40 | 41 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 42 | #include 43 | #include 44 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 45 | 46 | 47 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 55 | -------------------------------------------------------------------------------- /src/localization/localization_node.cpp: -------------------------------------------------------------------------------- 1 | /**\file localization_node.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | 9 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 10 | #include 11 | #include 12 | #include 13 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 14 | 15 | 16 | 17 | // ###################################################################################
############################################################################## 18 | int main(int argc, char** argv) { 19 | ros::init(argc, argv, "drl_localization_node"); 20 | 21 | ros::NodeHandlePtr node_handle(new ros::NodeHandle()); 22 | ros::NodeHandlePtr private_node_handle(new ros::NodeHandle("~")); 23 | 24 | std::string pcl_verbosity_level; 25 | private_node_handle->param("pcl_verbosity_level", pcl_verbosity_level, std::string("ERROR")); 26 | dynamic_robot_localization::verbosity_levels::setVerbosityLevelPCL(pcl_verbosity_level); 27 | 28 | std::string ros_verbosity_level; 29 | private_node_handle->param("ros_verbosity_level", ros_verbosity_level, std::string("INFO")); 30 | dynamic_robot_localization::verbosity_levels::setVerbosityLevelROS(ros_verbosity_level); 31 | 32 | std::string localization_point_type; 33 | private_node_handle->param("localization_point_type", localization_point_type, std::string("PointXYZRGBNormal")); 34 | 35 | // Note: Given that PointXYZRGBNormal is the most common used point cloud type, the other 2 were disabled to speedup compilation 36 | // For enabling them, update DRL_POINT_TYPES in common/common.h and uncomment the lines below 37 | //if (localization_point_type == "PointXYZRGBNormal") { 38 | ROS_INFO("Localization system using PointXYZRGBNormal point type"); 39 | dynamic_robot_localization::Localization localization; 40 | localization.setupConfigurationFromParameterServer(node_handle, private_node_handle, ""); 41 | localization.startLocalization(); 42 | /*} else if (localization_point_type == "PointXYZINormal") { 43 | ROS_INFO("Localization system using PointXYZINormal point type"); 44 | dynamic_robot_localization::Localization localization; 45 | localization.setupConfigurationFromParameterServer(node_handle, private_node_handle); 46 | localization.startLocalization(); 47 | } else { 48 | ROS_INFO("Localization system using PointNormal point type"); 49 | dynamic_robot_localization::Localization localization; 50 | localization.setupConfigurationFromParameterServer(node_handle, private_node_handle); 51 | localization.startLocalization(); 52 | }*/ 53 | 54 | return 0; 55 | } 56 | // ###################################################################################
############################################################################# 57 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/ia_ransac.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2011, Willow Garage, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of Willow Garage, Inc. nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * $Id$ 37 | * 38 | */ 39 | 40 | 41 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 42 | #include 43 | #include 44 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 45 | 46 | 47 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 55 | -------------------------------------------------------------------------------- /src/cloud_matchers/feature_matchers/sample_consensus_prerejective.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2011, Willow Garage, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of Willow Garage, Inc. nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * $Id$ 37 | * 38 | */ 39 | 40 | 41 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 42 | #include 43 | #include 44 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 45 | 46 | 47 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 55 | 56 | -------------------------------------------------------------------------------- /src/common/verbosity_levels.cpp: -------------------------------------------------------------------------------- 1 | /**\file verbosity_levels.cpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 11 | 12 | namespace dynamic_robot_localization { 13 | namespace verbosity_levels { 14 | 15 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 16 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 17 | 18 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 19 | bool setVerbosityLevelPCL(std::string level) { 20 | if (level == "VERBOSE") { 21 | pcl::console::setVerbosityLevel(pcl::console::L_VERBOSE); 22 | } else if (level == "DEBUG") { 23 | pcl::console::setVerbosityLevel(pcl::console::L_DEBUG); 24 | } else if (level == "INFO") { 25 | pcl::console::setVerbosityLevel(pcl::console::L_INFO); 26 | } else if (level == "WARN") { 27 | pcl::console::setVerbosityLevel(pcl::console::L_WARN); 28 | } else if (level == "ERROR") { 29 | pcl::console::setVerbosityLevel(pcl::console::L_ERROR); 30 | } else if (level == "ALWAYS") { 31 | pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); 32 | } else { 33 | return false; 34 | } 35 | 36 | return true; 37 | } 38 | 39 | bool setVerbosityLevelROS(std::string level) { 40 | if (level == "DEBUG") { 41 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { 42 | ros::console::notifyLoggerLevelsChanged(); 43 | } 44 | } else if (level == "INFO") { 45 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info)) { 46 | ros::console::notifyLoggerLevelsChanged(); 47 | } 48 | } else if (level == "WARN") { 49 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn)) { 50 | ros::console::notifyLoggerLevelsChanged(); 51 | } 52 | } else if (level == "ERROR") { 53 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error)) { 54 | ros::console::notifyLoggerLevelsChanged(); 55 | } 56 | } else if (level == "FATAL") { 57 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Fatal)) { 58 | ros::console::notifyLoggerLevelsChanged(); 59 | } 60 | } else { 61 | return false; 62 | } 63 | 64 | return true; 65 | } 66 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 67 | 68 | 69 | } /* namespace verbosity_levels */ 70 | } /* namespace dynamic_robot_localization */ 71 | -------------------------------------------------------------------------------- /include/dynamic_robot_localization/cloud_analyzers/impl/cloud_analyzer.hpp: -------------------------------------------------------------------------------- 1 | /**\file cloud_analyzer.hpp 2 | * \brief Description... 3 | * 4 | * @version 1.0 5 | * @author Carlos Miguel Correia da Costa 6 | */ 7 | 8 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 9 | #include 10 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 11 | 12 | namespace dynamic_robot_localization { 13 | 14 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 15 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 16 | 17 | // ============================================================================= ============================================================================ 18 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 19 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 20 | 21 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 22 | template 23 | double CloudAnalyzer::analyzeCloud(const tf2::Transform& estimated_pose, const pcl::PointCloud& pointcloud, std::vector& analysis_histogram_out) { 24 | if (pointcloud.empty()) { 25 | return 0.0; 26 | } 27 | 28 | if (computeAnalysisHistogram(estimated_pose, pointcloud, analysis_histogram_out)) { 29 | return computeAnalysis(analysis_histogram_out); 30 | } else { 31 | return -1.0; 32 | } 33 | } 34 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 35 | // ============================================================================= =========================================================================== 36 | 37 | // ============================================================================= ======================================================================= 38 | // ============================================================================= ======================================================================= 39 | 40 | 41 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 43 | 44 | } /* namespace dynamic_robot_localization */ 45 | -------------------------------------------------------------------------------- /install/a_dependencies.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | ros_version=${1:-"$(rosversion -d)"} 4 | install_args=${2:-"-y --allow-unauthenticated"} 5 | 6 | 7 | echo "####################################################################################################" 8 | echo "##### Checking and installing dependencies for dynamic_robot_localization ros package" 9 | echo "####################################################################################################" 10 | 11 | 12 | # required system dependencies 13 | sudo apt-get install coreutils ${install_args} 14 | sudo apt-get install git ${install_args} 15 | sudo apt-get install libeigen3-dev ${install_args} 16 | 17 | if [ ${ros_version} != 'noetic' ]; then 18 | sudo apt-get install python-wstool ${install_args} 19 | sudo apt-get install python-catkin-tools ${install_args} 20 | else 21 | sudo apt-get install python3-wstool ${install_args} 22 | sudo apt-get install python3-catkin-tools python3-osrf-pycommon ${install_args} 23 | fi 24 | 25 | 26 | # required ros packages 27 | sudo apt-get install ros-${ros_version}-angles ${install_args} 28 | sudo apt-get install ros-${ros_version}-cpp-common ${install_args} 29 | sudo apt-get install ros-${ros_version}-dynamic-reconfigure ${install_args} 30 | sudo apt-get install ros-${ros_version}-geometry-msgs ${install_args} 31 | sudo apt-get install ros-${ros_version}-map-server ${install_args} 32 | sudo apt-get install ros-${ros_version}-message-generation ${install_args} 33 | sudo apt-get install ros-${ros_version}-message-runtime ${install_args} 34 | sudo apt-get install ros-${ros_version}-nav-msgs ${install_args} 35 | sudo apt-get install ros-${ros_version}-rosconsole ${install_args} 36 | sudo apt-get install ros-${ros_version}-roscpp ${install_args} 37 | sudo apt-get install ros-${ros_version}-rostime ${install_args} 38 | sudo apt-get install ros-${ros_version}-sensor-msgs ${install_args} 39 | sudo apt-get install ros-${ros_version}-std-msgs ${install_args} 40 | sudo apt-get install ros-${ros_version}-tf2 ${install_args} 41 | sudo apt-get install ros-${ros_version}-tf2-msgs ${install_args} 42 | sudo apt-get install ros-${ros_version}-tf2-ros ${install_args} 43 | sudo apt-get install ros-${ros_version}-xmlrpcpp ${install_args} 44 | 45 | 46 | # optional ros packages for ukf filtering 47 | sudo apt-get install ros-${ros_version}-robot-localization ${install_args} 48 | 49 | 50 | # optional ros packages for dynamic map update 51 | sudo apt-get install ros-${ros_version}-dynamic-edt-3d ${install_args} 52 | sudo apt-get install ros-${ros_version}-octomap ${install_args} 53 | sudo apt-get install ros-${ros_version}-octomap-msgs ${install_args} 54 | sudo apt-get install ros-${ros_version}-octomap-ros ${install_args} 55 | sudo apt-get install ros-${ros_version}-octomap-rviz-plugins ${install_args} 56 | sudo apt-get install ros-${ros_version}-octovis ${install_args} 57 | 58 | 59 | # optional system dependencies for mesh_to_pointcloud package 60 | sudo apt-get install libopenscenegraph-dev ${install_args} 61 | sudo apt-get install libopenthreads-dev ${install_args} 62 | 63 | 64 | echo -e "\n\n" 65 | echo "----------------------------------------------------------------------------------------------------" 66 | echo ">>>>> Installation of dynamic_robot_localization dependencies finished" 67 | echo "----------------------------------------------------------------------------------------------------" 68 | -------------------------------------------------------------------------------- /include/dynamic_robot_localization/common/math_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /**\file math_utils.h 4 | * \brief Description... 5 | * 6 | * @version 1.0 7 | * @author Carlos Miguel Correia da Costa 8 | */ 9 | 10 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 11 | // std includes 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // ROS includes 18 | #include 19 | 20 | // PCL includes 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | // external libs includes 27 | #include 28 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 29 | 30 | namespace dynamic_robot_localization { 31 | 32 | // ############################################################################### math_utils ############################################################################## 33 | namespace math_utils { 34 | 35 | enum PointPosition { 36 | LEFT_LINE_SIDE, 37 | RIGHT_LINE_SIDE, 38 | ON_TOP_OF_LINE 39 | }; 40 | 41 | 42 | PointPosition computePointPositionInRelationToLinePoints( 43 | float line_start_point_x, float line_start_point_y, 44 | float line_end_point_x, float line_end_point_y, 45 | float query_point_x, float query_point_y); 46 | 47 | PointPosition computePointPositionInRelationToLine( 48 | float point_on_line_x, float point_on_line_y, 49 | float line_orientation_x, float line_orientation_y, 50 | float query_point_x, float query_point_y); 51 | 52 | PointPosition computePointPosition(float cross_product_z); 53 | 54 | void computeTransformationFromMatrices(const Eigen::Matrix4f& source_matrix, const Eigen::Matrix4f& target_matrix, Eigen::Matrix4f& transformation_matrix); 55 | 56 | 57 | bool isTransformValid(const tf2::Transform& transform); 58 | 59 | template 60 | bool isTransformValid(const Eigen::Matrix& transform); 61 | 62 | template 63 | std::string convertTransformToString(const Eigen::Matrix& transform, const std::string& line_prefix = "\n\t\t[ ", const std::string& line_suffix = " ]", const std::string& number_separator = " | "); 64 | 65 | template 66 | bool sortFunctionForPairSecondValueAscendingOrder(const std::pair &left, const std::pair &right) { return left.second < right.second; } 67 | 68 | template 69 | bool sortFunctionForPairSecondValueDescendingOrder(const std::pair &left, const std::pair &right) { return left.second > right.second; } 70 | 71 | void getRollPitchYawFromMatrix(const Eigen::Matrix4f& matrix, double& roll_out, double& pitch_out, double& yaw_out); 72 | void getRollPitchYawFromMatrixUsigTF2(const Eigen::Matrix4f& matrix, double& roll_out, double& pitch_out, double& yaw_out); 73 | 74 | 75 | } /* namespace math_utils */ 76 | } /* namespace dynamic_robot_localization */ 77 | 78 | 79 | #ifdef DRL_NO_PRECOMPILE 80 | #include 81 | #endif 82 | --------------------------------------------------------------------------------