├── .catkin_workspace ├── .gitignore ├── LICENSE ├── README ├── docs ├── project_description.pdf ├── project_description.tex └── thesis.pdf ├── doxygen └── Doxyfile └── src ├── CMakeLists.txt ├── binary_detector ├── CMakeLists.txt ├── cfg │ └── Parameters.cfg ├── include │ └── binary_detector │ │ ├── binary_detector.h │ │ ├── groupbox.h │ │ ├── observed_parameter_manager.h │ │ ├── parameter_manager.h │ │ └── reconfigure_parameter_manager.h ├── package.xml ├── params │ └── pink_pad.yaml └── src │ ├── binary_detector.cpp │ ├── groupbox.cpp │ ├── observed_parameter_manager.cpp │ ├── parameter_manager.cpp │ └── reconfigure_parameter_manager.cpp ├── distance_filter ├── CMakeLists.txt ├── cfg │ └── Parameters.cfg ├── include │ └── distance_filter │ │ ├── distance_filter.h │ │ ├── groupbox.h │ │ ├── observed_parameter_manager.h │ │ ├── parameter_manager.h │ │ └── reconfigure_parameter_manager.h ├── package.xml └── src │ ├── distance_filter.cpp │ ├── groupbox.cpp │ ├── observed_parameter_manager.cpp │ ├── parameter_manager.cpp │ └── reconfigure_parameter_manager.cpp ├── feature_detector ├── CMakeLists.txt ├── cfg │ ├── Parameters.cfg │ └── ParametersOld.cfg ├── include │ └── feature_detector │ │ ├── descriptor_matcher_groupbox.h │ │ ├── feature_detector.h │ │ ├── groupbox.h │ │ ├── observed_parameter_manager.h │ │ ├── parameter_manager.h │ │ ├── reconfigure_parameter_manager.h │ │ ├── surf_groupbox.h │ │ └── thresholds_groupbox.h ├── package.xml └── src │ ├── descriptor_matcher_groupbox.cpp │ ├── feature_detector.cpp │ ├── groupbox.cpp │ ├── observed_parameter_manager.cpp │ ├── parameter_manager.cpp │ ├── reconfigure_parameter_manager.cpp │ ├── surf_groupbox.cpp │ └── thresholds_groupbox.cpp ├── hsv_filter ├── CMakeLists.txt ├── cfg │ └── Parameters.cfg ├── include │ └── hsv_filter │ │ ├── groupbox.h │ │ ├── hsv_filter.h │ │ ├── observed_parameter_manager.h │ │ ├── parameter_manager.h │ │ └── reconfigure_parameter_manager.h ├── launch │ └── hsv_filter.launch ├── package.xml ├── params │ ├── body_milk.yaml │ ├── book.yml │ └── pink_pad.yaml └── src │ ├── groupbox.cpp │ ├── hsv_filter.cpp │ ├── observed_parameter_manager.cpp │ ├── parameter_manager.cpp │ └── reconfigure_parameter_manager.cpp ├── morphology_filter ├── CMakeLists.txt ├── cfg │ └── Parameters.cfg ├── include │ └── morphology_filter │ │ ├── groupbox.h │ │ ├── morphology_filter.h │ │ ├── observed_parameter_manager.h │ │ ├── parameter_manager.h │ │ └── reconfigure_parameter_manager.h ├── package.xml ├── params │ └── pink_pad.yaml └── src │ ├── groupbox.cpp │ ├── morphology_filter.cpp │ ├── observed_parameter_manager.cpp │ ├── parameter_manager.cpp │ └── reconfigure_parameter_manager.cpp ├── object_detection ├── CMakeLists.txt ├── include │ └── object_detection │ │ ├── observer.h │ │ └── subject.h └── package.xml ├── object_detection_2d ├── CMakeLists.txt ├── include │ └── object_detection_2d │ │ ├── definition_director.h │ │ ├── detection_director.h │ │ ├── detector.h │ │ ├── filter.h │ │ ├── gui.h │ │ ├── interactive_image.h │ │ ├── parameter_storage.h │ │ ├── parametrizable.h │ │ ├── ros_detector_wrapper.h │ │ └── ros_filter_wrapper.h ├── msg │ ├── MouseEvent.msg │ ├── Point2D.msg │ └── Rect2D.msg ├── package.xml └── src │ ├── definition_director.cpp │ ├── detection_director.cpp │ ├── gui.cpp │ ├── interactive_image.cpp │ ├── parameter_storage.cpp │ ├── ros_detector_wrapper.cpp │ └── ros_filter_wrapper.cpp ├── object_detection_2d_msgs ├── CMakeLists.txt ├── msg │ ├── DetectedObject2D.msg │ └── DetectedObject2DArray.msg └── package.xml ├── object_detection_2d_nodes ├── CMakeLists.txt ├── launch │ ├── feature_gui_definition.launch │ ├── feature_gui_detection.launch │ ├── feature_node_definition.launch │ ├── feature_node_detection.launch │ ├── hsv_definition.launch │ ├── hsv_detection.launch │ ├── hsv_morph_binary_nodes_definition.launch │ ├── hsv_morph_binary_nodes_pinkpad.launch │ └── hsv_pinkpad_detection.launch ├── package.xml ├── params │ ├── feature_ros_book.xml │ ├── feature_ros_book_dynparam.yaml │ ├── feature_ros_book_selection.yaml │ └── hsv_pinkpad.xml ├── rviz │ ├── feature_detection.rviz │ └── hsv_morph_binary_nodes.rviz └── src │ ├── binary_detector_node.cpp │ ├── feature_definition.cpp │ ├── feature_detection.cpp │ ├── feature_detector_node.cpp │ ├── hsv_definition.cpp │ ├── hsv_detection.cpp │ ├── hsv_filter_node.cpp │ └── morphology_filter_node.cpp ├── object_detection_2d_vis ├── CMakeLists.txt ├── include │ └── object_detection_2d_vis │ │ └── visualizer.h ├── launch │ ├── vis_area_selection.launch │ └── visualizer.launch ├── package.xml └── src │ ├── area_selection.cpp │ ├── visualizer.cpp │ └── visualizer_main.cpp ├── object_detection_3d ├── CMakeLists.txt ├── include │ └── object_detection_3d │ │ ├── definition_director.h │ │ ├── detection_director.h │ │ ├── detector.h │ │ ├── filter.h │ │ ├── gui.h │ │ ├── parameter_storage.h │ │ ├── parametrizable.h │ │ ├── pcl_types.h │ │ ├── ros_detector_wrapper.h │ │ └── ros_filter_wrapper.h ├── package.xml └── src │ ├── definition_director.cpp │ ├── detection_director.cpp │ ├── gui.cpp │ ├── parameter_storage.cpp │ ├── ros_detector_wrapper.cpp │ └── ros_filter_wrapper.cpp ├── object_detection_3d_msgs ├── CMakeLists.txt ├── msg │ ├── DetectedObject3D.msg │ ├── DetectedObject3DArray.msg │ └── OrientedBox.msg └── package.xml ├── object_detection_3d_nodes ├── CMakeLists.txt ├── launch │ ├── distance_filter_node.launch │ ├── distance_shape_definition.launch │ ├── distance_shape_detection.launch │ ├── distance_shape_nodes_definition.launch │ └── shape_detector_node.launch ├── package.xml ├── rviz │ └── distance_shape_nodes.rviz └── src │ ├── distance_filter_node.cpp │ ├── distance_shape_definition.cpp │ ├── distance_shape_detection.cpp │ └── shape_detector_node.cpp ├── object_painter ├── CMakeLists.txt ├── cfg │ └── ObjectPainter.cfg ├── include │ └── object_painter │ │ ├── object_painter.h │ │ └── parameter_manager.h ├── launch │ └── object_painter.launch ├── package.xml └── src │ ├── main.cpp │ └── object_painter.cpp ├── objects2d_to_objects3d ├── CMakeLists.txt ├── launch │ ├── objects2d_to_objects3d.launch │ └── objects2d_to_objects3d_test.launch ├── package.xml └── src │ └── objects2d_to_objects3d.cpp ├── objects_to_markers ├── CMakeLists.txt ├── cfg │ └── Parameters.cfg ├── include │ └── objects_to_markers │ │ ├── objects_to_markers.h │ │ └── parameter_manager.h ├── launch │ └── objects_to_markers.launch ├── package.xml └── src │ ├── main.cpp │ ├── marker_publisher_test.cpp │ └── objects_to_markers.cpp └── shape_detector ├── CMakeLists.txt ├── cfg └── Parameters.cfg ├── include └── shape_detector │ ├── groupbox.h │ ├── normal_estimation_groupbox.h │ ├── object_computer.h │ ├── observed_parameter_manager.h │ ├── parameter_manager.h │ ├── reconfigure_parameter_manager.h │ ├── segmentation_groupbox.h │ ├── shape_detector.h │ └── shape_enum.h ├── package.xml ├── rename.bash └── src ├── cylinder_computer.cpp ├── groupbox.cpp ├── normal_estimation_groupbox.cpp ├── object_computer.cpp ├── observed_parameter_manager.cpp ├── parameter_manager.cpp ├── reconfigure_parameter_manager.cpp ├── segmentation_groupbox.cpp ├── shape_detector.cpp └── sphere_computer.cpp /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore autogenerated .catkin_workspace file. 2 | /.catkin_workspace 3 | 4 | # Ignore folders that are autogenerated in ROS. 5 | build/ 6 | devel/ 7 | 8 | # Ignore autogenerated doxygen documentation. 9 | doxygen/* 10 | !doxygen/Doxyfile 11 | 12 | # Ignore autogenerated tex-files and images in documentation. 13 | docs/* 14 | !docs/project_description.tex 15 | !docs/project_description.pdf 16 | !docs/thesis.pdf 17 | 18 | # Ignore videos. 19 | video/ 20 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Janosch Hoffmann 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README: -------------------------------------------------------------------------------- 1 | !! NOTE !! 2 | ========== 3 | This project is deprecated. 4 | I've created this project in 2017 for my master thesis. Since then, no signifiant changes have been made. 5 | Since I'm not working with ROS anymore, I will probably not be able to answer your questions. 6 | Feel free to use the code and concepts as building blocks or as an inspiration for your own project. 7 | Don't expect this project to run by "plug and play" or to even compile with current versions of ROS. 8 | 9 | 10 | Summary 11 | ======= 12 | A simple "framework" for ROS-based 2D and 3D object recognition software. 13 | 14 | Documentation 15 | ============= 16 | You find a description of the project in docs/project_description.pdf. 17 | docs/thesis.pdf contains a more detailed description but it's not up-to-date 18 | anymore. 19 | The following video provides an overview and demonstrations: 20 | https://www.youtube.com/watch?v=Pmur4kxrexE 21 | 22 | Requirements 23 | ============ 24 | ROS 25 | OpenCV 3 (only for 2D object recognition) 26 | PCL 1.7 (only for 3D object recognition) 27 | Eigen 3 (only for 3D object recognition) 28 | Qt5 (not necessary if you do not compile the GUI-files) 29 | 30 | Compilation 31 | =========== 32 | 1. Follow ROS installation (Desktop Full Install): http://wiki.ros.org/kinetic/Installation/Ubuntu 33 | I needed to enable mysql (sudo systemctl enable mysql) for this to work. 34 | 2. Create catkin workspace: 35 | mkdir -p ~/catkin_ws/src 36 | cd ~/catkin_ws 37 | catkin_make 38 | 3. Clone project: 39 | cd ~ 40 | git clone https://github.com/joffman/ros_object_recognition.git 41 | 4. Copy/Move src folder into your workspace. 42 | cp -r ~/ros_object_recognition/src/* ~/catkin_ws/src/ 43 | 5. Compile: 44 | cd ~/catkin_ws 45 | catkin_make 46 | 47 | License 48 | ======= 49 | MIT license 50 | -------------------------------------------------------------------------------- /docs/project_description.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/joffman/ros_object_recognition/46437d4a6651ac150fae2dec19dfd4e149f975ae/docs/project_description.pdf -------------------------------------------------------------------------------- /docs/thesis.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/joffman/ros_object_recognition/46437d4a6651ac150fae2dec19dfd4e149f975ae/docs/thesis.pdf -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/binary_detector/cfg/Parameters.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "binary_detector" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("object_name", str_t, 0, \ 9 | "Name that is given to detected objects", "") 10 | 11 | LENGTH_FRAC_LOWER_LIMIT = 0.005 12 | LENGTH_FRAC_UPPER_LIMIT = 3. 13 | gen.add("min_length_fraction", double_t, 0, \ 14 | "Only contours that have at least this fraction of (image-width + " + \ 15 | "image-height) are detected", \ 16 | 0.05, LENGTH_FRAC_LOWER_LIMIT, LENGTH_FRAC_UPPER_LIMIT) 17 | gen.add("max_length_fraction", double_t, 0, \ 18 | "Only contours that have at most this fraction of (image-width + " + \ 19 | "image-height) are detected", \ 20 | 0.4, LENGTH_FRAC_LOWER_LIMIT, LENGTH_FRAC_UPPER_LIMIT) 21 | 22 | exit(gen.generate(PACKAGE, "binary_detector", "Parameters")) 23 | -------------------------------------------------------------------------------- /src/binary_detector/include/binary_detector/groupbox.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the GroupBox class. 3 | */ 4 | 5 | #ifndef BINARY_DETECTOR_GROUPBOX_H 6 | #define BINARY_DETECTOR_GROUPBOX_H 7 | 8 | // TODO: Add widget for object-name. 9 | 10 | // Qt. 11 | #include 12 | 13 | // Our headers. 14 | #include 15 | 16 | // Forward declarations. 17 | class QWidget; 18 | class QLabel; 19 | class QSlider; 20 | class QLCDNumber; 21 | 22 | 23 | namespace binary_detector { 24 | 25 | class ObservedParameterManager; 26 | 27 | /** \brief QGroupBox for accessing the parameters of a BinaryDetector object. 28 | */ 29 | class GroupBox : public QGroupBox, 30 | public object_detection::Observer { 31 | Q_OBJECT 32 | 33 | public: 34 | /** \brief Creates and initializes the GroupBox. 35 | * 36 | * \param[in] parent Pointer to QWidget that serves as a parent of the 37 | * GroupBox. 38 | */ 39 | explicit GroupBox(QWidget* parent = nullptr); 40 | 41 | /** \brief Updates the view of the GroupBox so that it displays the current 42 | * values of the parameters. 43 | */ 44 | void update() override; 45 | 46 | /** \brief Sets the parameter-manager of the BinaryDetector. 47 | * 48 | * The ObservedParameterManager manages the parameters of a BinaryDetector 49 | * object. 50 | * The manager represents the Model of the MVC-pattern, while the GroupBox 51 | * represents the View and the Controller. 52 | */ 53 | void setParameterManager(ObservedParameterManager* const); 54 | 55 | /** \brief Number of ticks of each slider in the GroupBox. */ 56 | static constexpr int num_slider_ticks {1000}; 57 | // TODO: I don't like that it's public; necessary for use by helper functions. 58 | 59 | signals: 60 | void minLCDSignal(double); 61 | void maxLCDSignal(double); 62 | 63 | private slots: 64 | void requestMinLengthChange(const int) const; 65 | void requestMaxLengthChange(const int) const; 66 | 67 | private: 68 | ObservedParameterManager* param_manager_ {nullptr}; 69 | 70 | QLabel* min_length_label_; 71 | QSlider* min_length_slider_; 72 | QLCDNumber* min_length_lcd_; 73 | 74 | QLabel* max_length_label_; 75 | QSlider* max_length_slider_; 76 | QLCDNumber* max_length_lcd_; 77 | }; 78 | 79 | } // binary_detector 80 | 81 | #endif // BINARY_DETECTOR_GROUPBOX_H 82 | -------------------------------------------------------------------------------- /src/binary_detector/include/binary_detector/observed_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ObservedParameterManager class. 3 | */ 4 | 5 | #ifndef BINARY_DETECTOR_OBSERVED_PARAMETER_MANAGER_H 6 | #define BINARY_DETECTOR_OBSERVED_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include 10 | 11 | // Headers of this project. 12 | #include 13 | #include 14 | #include "parameter_manager.h" 15 | 16 | 17 | namespace cv { 18 | class FileStorage; 19 | } 20 | 21 | namespace binary_detector { 22 | 23 | /** \brief Observable parameter-manager for a BinaryDetector object. 24 | * 25 | * This ParameterManager inherits from object_detection:Subject, 26 | * making it possible to register observers for it. 27 | * It also inherits from object_detection_2d::Parametrizable. 28 | * 29 | * Using this parameter-manager makes it possible to access the parameters 30 | * of a BinaryDetector via the object_detection_2d::GUI. 31 | */ 32 | class ObservedParameterManager : public ParameterManager, 33 | public object_detection::Subject, 34 | public object_detection_2d::Parametrizable { 35 | public: 36 | // 37 | // Parametrizable interface. 38 | // 39 | 40 | /** \brief Writes all parameters to the given FileStorage object. */ 41 | void writeParametersToStorage(cv::FileStorage&) const override; 42 | 43 | /** \brief Reads all parameters from the given FileStorage object 44 | * and sets its parameters accordingly. 45 | */ 46 | void setParametersFromStorage(cv::FileStorage&) override; 47 | 48 | // 49 | // Setters. 50 | // 51 | 52 | /** \brief Sets the name of detected objects. */ 53 | void setObjectName(const std::string&); 54 | 55 | 56 | /** \brief Sets the minimum-length-fraction parameter. */ 57 | void setMinLengthFraction(double); 58 | 59 | /** \brief Sets the maximum-length-fraction parameter. */ 60 | void setMaxLengthFraction(double); 61 | }; 62 | 63 | } // binary_detector 64 | 65 | #endif // BINARY_DETECTOR_OBSERVED_PARAMETER_MANAGER_H 66 | -------------------------------------------------------------------------------- /src/binary_detector/include/binary_detector/parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ParameterManager class. 3 | */ 4 | 5 | #ifndef BINARY_DETECTOR_PARAMETER_MANAGER_H 6 | #define BINARY_DETECTOR_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include 10 | 11 | 12 | namespace binary_detector { 13 | 14 | /** \brief Parameter-manager for BinaryDetector objects. 15 | * 16 | * A ParameterManager object manages the parameters of 17 | * a BinaryDetector object and provides getter- and setter-methods for 18 | * accessing those parameters. 19 | */ 20 | class ParameterManager { 21 | public: 22 | /** \brief Lower limit for the length-fraction-parameters. */ 23 | static constexpr double length_fraction_lower_limit {1. / 200.}; 24 | /** \brief Upper limit for the length-fraction-parameters. */ 25 | static constexpr double length_fraction_upper_limit {3.}; 26 | 27 | // 28 | // Getters. 29 | // 30 | 31 | /** \brief Returns the name that is given to detected objects. */ 32 | std::string objectName() const { return object_name_; } 33 | 34 | /** \brief Returns the value of the minimum-length-fraction parameter. */ 35 | double minLengthFraction() const { return min_length_fraction_; } 36 | /** \brief Returns the value of the maximum-length-fraction parameter. */ 37 | double maxLengthFraction() const { return max_length_fraction_; } 38 | 39 | // 40 | // Setters. 41 | // 42 | 43 | /** \brief Sets the name that is given to detected objects. */ 44 | void setObjectName(const std::string&); 45 | 46 | /** \brief Sets the value of the minimum-length-fraction parameter. */ 47 | void setMinLengthFraction(double); 48 | /** \brief Sets the value of the maximum-length-fraction parameter. */ 49 | void setMaxLengthFraction(double); 50 | 51 | private: 52 | std::string object_name_; 53 | 54 | double min_length_fraction_ {1. / 20.}; 55 | double max_length_fraction_ {1. / 3.}; 56 | }; 57 | 58 | } // binary_detector 59 | 60 | #endif // BINARY_DETECTOR_PARAMETER_MANAGER_H 61 | -------------------------------------------------------------------------------- /src/binary_detector/include/binary_detector/reconfigure_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ReconfigureParameterManager class. 3 | */ 4 | 5 | #ifndef BINARY_DETECTOR_RECONFIGURE_PARAMETER_MANAGER_H 6 | #define BINARY_DETECTOR_RECONFIGURE_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include 10 | 11 | // dynamic_reconfigure. 12 | #include 13 | #include 14 | 15 | // Headers of this project. 16 | #include "parameter_manager.h" 17 | 18 | 19 | namespace binary_detector { 20 | 21 | /** \brief dynamic_reconfigure parameter-manager for BinaryDetector objects. 22 | * 23 | * A ReconfigureParameterManager object allows to manage the parameters 24 | * of a BinaryDetector object via a corresponding set of dynamic_reconfigure 25 | * parameters. 26 | */ 27 | class ReconfigureParameterManager : public ParameterManager { 28 | public: 29 | /** \brief Initializes the dynamic_reconfigure server and registers a callback 30 | * for this server. 31 | */ 32 | ReconfigureParameterManager(); 33 | 34 | private: 35 | void reconfigureCallback(ParametersConfig& config, uint32_t); 36 | 37 | // dynamic_reconfigure server & callback. 38 | dynamic_reconfigure::Server reconfigure_server_; 39 | dynamic_reconfigure::Server::CallbackType 40 | reconfigure_callback_; 41 | }; 42 | 43 | } // binary_detector 44 | 45 | #endif // BINARY_DETECTOR_RECONFIGURE_PARAMETER_MANAGER_H 46 | -------------------------------------------------------------------------------- /src/binary_detector/params/pink_pad.yaml: -------------------------------------------------------------------------------- 1 | !!python/object/new:dynamic_reconfigure.encoding.Config 2 | dictitems: 3 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 4 | dictitems: 5 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 6 | state: [] 7 | id: 0 8 | max_length_fraction: 2.6355999999999997 9 | min_length_fraction: 0.6588999999999999 10 | name: Default 11 | object_name: '' 12 | parameters: !!python/object/new:dynamic_reconfigure.encoding.Config 13 | state: [] 14 | parent: 0 15 | state: true 16 | type: '' 17 | state: [] 18 | max_length_fraction: 2.6355999999999997 19 | min_length_fraction: 0.6588999999999999 20 | object_name: '' 21 | state: [] 22 | -------------------------------------------------------------------------------- /src/binary_detector/src/observed_parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ObservedParameterManager class. 3 | */ 4 | 5 | // std. 6 | #include 7 | 8 | // OpenCV. 9 | #include // FileStorage 10 | 11 | // Headers of this package. 12 | #include 13 | 14 | using namespace std; 15 | 16 | 17 | namespace { 18 | 19 | string fullParamName(const string& param_name) 20 | { 21 | return "BinaryDetector-" + param_name; 22 | } 23 | 24 | } // anonymous namespace 25 | 26 | 27 | namespace binary_detector { 28 | 29 | void ObservedParameterManager::writeParametersToStorage(cv::FileStorage& fs) 30 | const 31 | { 32 | auto& fpn = fullParamName; 33 | fs << fpn("min_length_frac") << minLengthFraction() 34 | << fpn("max_length_frac") << maxLengthFraction(); 35 | } 36 | 37 | 38 | void ObservedParameterManager::setParametersFromStorage(cv::FileStorage& fs) 39 | { 40 | auto& fpn = fullParamName; 41 | 42 | { 43 | auto fn = fs[fullParamName("min_length_frac")]; 44 | if (!fn.empty()) 45 | setMinLengthFraction((double)fn); 46 | } 47 | 48 | { 49 | auto fn = fs[fullParamName("max_length_frac")]; 50 | if (!fn.empty()) 51 | setMaxLengthFraction((double)fn); 52 | } 53 | } 54 | 55 | 56 | void ObservedParameterManager::setObjectName(const string& name) 57 | { 58 | ParameterManager::setObjectName(name); 59 | notify(); 60 | } 61 | 62 | 63 | void ObservedParameterManager::setMinLengthFraction(double val) 64 | { 65 | ParameterManager::setMinLengthFraction(val); 66 | notify(); 67 | } 68 | 69 | 70 | void ObservedParameterManager::setMaxLengthFraction(double val) 71 | { 72 | ParameterManager::setMaxLengthFraction(val); 73 | notify(); 74 | } 75 | 76 | } // binary_detector 77 | -------------------------------------------------------------------------------- /src/binary_detector/src/parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ParameterManager. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include // min(), max() 8 | 9 | // binary_detector. 10 | #include 11 | 12 | 13 | namespace { 14 | 15 | template 16 | inline T truncateToRange(T val, T min, T max) 17 | { 18 | return std::min(std::max(val, min), max); 19 | } 20 | 21 | } // anonymous ns 22 | 23 | 24 | namespace binary_detector { 25 | 26 | constexpr double ParameterManager::length_fraction_lower_limit; 27 | constexpr double ParameterManager::length_fraction_upper_limit; 28 | 29 | void ParameterManager::setObjectName(const std::string& name) 30 | { 31 | object_name_ = name; 32 | } 33 | 34 | void ParameterManager::setMinLengthFraction(double val) 35 | { 36 | min_length_fraction_ = truncateToRange(val, length_fraction_lower_limit, 37 | length_fraction_upper_limit); 38 | } 39 | 40 | void ParameterManager::setMaxLengthFraction(double val) 41 | { 42 | max_length_fraction_ = truncateToRange(val, length_fraction_lower_limit, 43 | length_fraction_upper_limit); 44 | } 45 | 46 | } // binary_detector 47 | -------------------------------------------------------------------------------- /src/binary_detector/src/reconfigure_parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ReconfigureParameterManager. 3 | */ 4 | 5 | // std. 6 | #include // bind(), placeholders 7 | 8 | // binary_detector. 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | 15 | namespace binary_detector { 16 | 17 | ReconfigureParameterManager::ReconfigureParameterManager() 18 | { 19 | reconfigure_callback_ = bind(&ReconfigureParameterManager::reconfigureCallback, this, 20 | placeholders::_1, placeholders::_2); 21 | reconfigure_server_.setCallback(reconfigure_callback_); 22 | } 23 | 24 | 25 | void ReconfigureParameterManager::reconfigureCallback(ParametersConfig& config, uint32_t) 26 | { 27 | setObjectName(config.object_name); 28 | 29 | setMinLengthFraction(config.min_length_fraction); 30 | setMaxLengthFraction(config.max_length_fraction); 31 | } 32 | 33 | } // binary_detector 34 | -------------------------------------------------------------------------------- /src/distance_filter/cfg/Parameters.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "distance_filter" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("min_distance", double_t, 0, 9 | "Points with a smaller distance are filtered out", 10 | 1.0, 0, 10.0) 11 | gen.add("max_distance", double_t, 0, 12 | "Points with a greater distance are filtered out", 13 | 1.5, 0, 10.0) 14 | 15 | exit(gen.generate(PACKAGE, "distance_filter", "Parameters")) 16 | -------------------------------------------------------------------------------- /src/distance_filter/include/distance_filter/distance_filter.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the DistanceFilter class. 3 | */ 4 | 5 | #ifndef DISTANCE_FILTER_DISTANCE_FILTER_H 6 | #define DISTANCE_FILTER_DISTANCE_FILTER_H 7 | 8 | // PCL. 9 | #include 10 | 11 | // Our headers. 12 | #include 13 | #include 14 | 15 | 16 | namespace distance_filter { 17 | 18 | class ParameterManager; 19 | 20 | 21 | /** \brief Filter for filtering out all points of a point cloud with a 22 | * distance outside an adjustable range. 23 | */ 24 | class DistanceFilter : public object_detection_3d::Filter { 25 | using PointT = object_detection_3d::PointT; 26 | using PointCloudT = object_detection_3d::PointCloudT; 27 | 28 | public: 29 | /** \brief Creates a DistanceFilter and initializes its parameter-manager. 30 | * 31 | * \param[in] Pointer to a valid ParameterManager. 32 | */ 33 | explicit DistanceFilter(ParameterManager* pm); 34 | 35 | /** \brief Filters the given point cloud. 36 | * 37 | * The point cloud is filtered so that only those points fulfilling the 38 | * condition 'min_distance <= distance <= max_distance' are retained. 39 | * 40 | * \param[in] cloud Point cloud that is filtered. 41 | * \return Point cloud containing only the points that passed through the 42 | * filter. 43 | * 44 | * \note The distance-limits are maintained by the ParameterManager member. 45 | */ 46 | PointCloudT::Ptr filter(const PointCloudT::ConstPtr& cloud) override; 47 | 48 | private: 49 | pcl::PassThrough createPassThroughForCloud( 50 | const PointCloudT::ConstPtr& cloud) const; 51 | 52 | ParameterManager* param_manager_; 53 | }; 54 | 55 | } // distance_filter 56 | 57 | #endif // DISTANCE_FILTER_DISTANCE_FILTER_H 58 | -------------------------------------------------------------------------------- /src/distance_filter/include/distance_filter/groupbox.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the GroupBox class. 3 | */ 4 | 5 | #ifndef DISTANCE_FILTER_GROUPBOX_H 6 | #define DISTANCE_FILTER_GROUPBOX_H 7 | 8 | // Qt. 9 | #include 10 | #include 11 | #include 12 | 13 | // Our headers. 14 | #include 15 | 16 | // Forward declarations. 17 | class QLabel; 18 | class QDoubleSpinBox; 19 | 20 | 21 | namespace distance_filter { 22 | 23 | class ObservedParameterManager; 24 | 25 | 26 | /** \brief GroupBox for accessing the parameters of a DistanceFilter. 27 | */ 28 | class GroupBox : public QGroupBox, public object_detection::Observer { 29 | Q_OBJECT 30 | 31 | public: 32 | /** \brief Creates and initializes a GroupBox. 33 | * 34 | * \param[in] parent Pointer to QWidget that serves as a parent of the 35 | * GroupBox. 36 | */ 37 | explicit GroupBox(QWidget* parent = nullptr); 38 | 39 | /** \brief Updates the view of the GroupBox so that it displays the current 40 | * values of the parameters. 41 | */ 42 | void update() override; 43 | 44 | /** \brief Sets the parameter-manager of the DistanceFilter. 45 | * 46 | * The ObservedParameterManager manages the parameters of a DistanceFilter 47 | * object. 48 | * The manager represents the Model of the MVC-pattern, while the GroupBox 49 | * represents the View and the Controller. 50 | */ 51 | void setParameterManager(ObservedParameterManager* filter); 52 | 53 | private slots: 54 | void requestMinDistanceChange(double) const; 55 | void requestMaxDistanceChange(double) const; 56 | 57 | private: 58 | void createDistanceLabels(); // or 'initDistanceLabels'?! 59 | void createDistanceSpinBoxes(); 60 | QGridLayout* createMainLayout(); 61 | 62 | // Data members. 63 | ObservedParameterManager* param_manager_ {nullptr}; 64 | 65 | QLabel* min_distance_label_; 66 | QLabel* max_distance_label_; 67 | QDoubleSpinBox* min_distance_spinbox_; 68 | QDoubleSpinBox* max_distance_spinbox_; 69 | }; 70 | 71 | } // distance_filter 72 | 73 | #endif // DISTANCE_FILTER_GROUPBOX_H 74 | -------------------------------------------------------------------------------- /src/distance_filter/include/distance_filter/observed_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ObservedParameterManager class. 3 | */ 4 | 5 | #ifndef DISTANCE_FILTER_OBSERVED_PARAMETER_MANAGER_H 6 | #define DISTANCE_FILTER_OBSERVED_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include 10 | 11 | // Our headers. 12 | #include 13 | #include 14 | #include "parameter_manager.h" 15 | 16 | 17 | namespace distance_filter { 18 | 19 | /** \brief Observable parameter-manager for a DistanceFilter object. 20 | * 21 | * This ParameterManager inherits from object_detection:Subject, 22 | * making it possible to register observers for it. 23 | * It also inherits from object_detection_3d::Parametrizable. 24 | * 25 | * Using this parameter-manager makes it possible to access the parameters 26 | * of an DistanceFilter via the object_detection_3d::GUI. 27 | */ 28 | class ObservedParameterManager : public ParameterManager, 29 | public object_detection::Subject, 30 | public object_detection_3d::Parametrizable { 31 | 32 | using MultiClassParameterMap = Parametrizable::MultiClassParameterMap; 33 | using SingleClassParameterMap = Parametrizable::SingleClassParameterMap; 34 | 35 | public: 36 | // 37 | // Parametrizable interface. 38 | // 39 | 40 | /** Creates a parameter-map that contains the values of all parameters. */ 41 | MultiClassParameterMap createParameterMap() const override; 42 | 43 | /** Sets all parameters to the values found in the given parameter-map. */ 44 | void setParametersFromMap(const MultiClassParameterMap&) override; 45 | 46 | // 47 | // Setters. 48 | // 49 | 50 | /** Sets the minimum-distance parameter. */ 51 | void setMinDistance(double); 52 | 53 | /** Sets the maximum-distance parameter. */ 54 | void setMaxDistance(double); 55 | 56 | private: 57 | void setParametersFromSingleClassMap( 58 | const SingleClassParameterMap& parameters); 59 | void setDistancesFromStrings( // maybe better call it 'setParameters...'?! 60 | const std::string& min_dist_str, const std::string& max_dist_str); 61 | 62 | bool trySetDistances(const double min_dist, const double max_dist); 63 | }; 64 | 65 | } // distance_filter 66 | 67 | #endif // DISTANCE_FILTER_OBSERVED_PARAMETER_MANAGER_H 68 | -------------------------------------------------------------------------------- /src/distance_filter/include/distance_filter/parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ParameterManager class. 3 | */ 4 | 5 | #ifndef DISTANCE_FILTER_PARAMETER_MANAGER_H 6 | #define DISTANCE_FILTER_PARAMETER_MANAGER_H 7 | 8 | 9 | namespace distance_filter { 10 | 11 | /** \brief Class for maintaining the parameters of DistanceFilter objects. 12 | * 13 | * The parameters are the minimum and the maximum distance of points that 14 | * pass through the filter. 15 | */ 16 | class ParameterManager { 17 | public: 18 | /** \brief Lower limit of the distance parameters. */ 19 | static constexpr double distance_lower_limit {0.}; 20 | /** \brief Upper limit of the distance parameters. */ 21 | static constexpr double distance_upper_limit {10.}; 22 | 23 | // 24 | // Getters & Setters. 25 | // 26 | /** \brief Returns the minimum-distance parameter. */ 27 | double minDistance() const { return min_distance_; } 28 | /** \brief Sets the minimum-distance parameter. 29 | * 30 | * \note The value is truncated to the valid range, given by the 31 | * distance_lower_limit and distance_upper_limit constants. 32 | */ 33 | void setMinDistance(double); 34 | 35 | /** \brief Returns the maximum-distance parameter. */ 36 | double maxDistance() const { return max_distance_; } 37 | /** \brief Sets the maximum-distance parameter. 38 | * 39 | * \note The value is truncated to the valid range, given by the 40 | * distance_lower_limit and distance_upper_limit constants. 41 | */ 42 | void setMaxDistance(double); 43 | 44 | /** Sets the minimum and the maximum distance parameters. 45 | * 46 | * \note The arguments have to fulfill the condition 'distance_lower_limit <= 47 | * min_distance <= max_distance <= distance_upper_limit'. 48 | * Otherwise a runtime_error is thrown. 49 | */ 50 | void setDistances(double min_distance, double max_distance); 51 | 52 | private: 53 | double min_distance_ {1.}; 54 | double max_distance_ {1.5}; 55 | }; 56 | 57 | } // distance_filter 58 | 59 | #endif // DISTANCE_FILTER_PARAMETER_MANAGER_H 60 | -------------------------------------------------------------------------------- /src/distance_filter/include/distance_filter/reconfigure_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ReconfigureParameterManager class. 3 | */ 4 | 5 | #ifndef DISTANCE_FILTER_RECONFIGURE_PARAMETER_MANAGER_H 6 | #define DISTANCE_FILTER_RECONFIGURE_PARAMETER_MANAGER_H 7 | 8 | // dynamic_reconfigure. 9 | #include 10 | #include 11 | 12 | // Our headers. 13 | #include "parameter_manager.h" 14 | 15 | 16 | namespace distance_filter { 17 | 18 | /** \brief dynamic_reconfigure parameter-manager for DistanceFilter objects. 19 | * 20 | * A ReconfigureParameterManager object allows to manage the parameters 21 | * of a DistanceFilter object via a corresponding set of dynamic_reconfigure 22 | * parameters. 23 | */ 24 | class ReconfigureParameterManager : public ParameterManager { 25 | public: 26 | /** \brief Initializes the dynamic_reconfigure server and registers a callback 27 | * for this server. 28 | */ 29 | ReconfigureParameterManager(); 30 | 31 | private: 32 | void reconfigureCallback(ParametersConfig& config, uint32_t); 33 | 34 | // dynamic_reconfigure server & callback. 35 | dynamic_reconfigure::Server reconfigure_server_; 36 | dynamic_reconfigure::Server::CallbackType 37 | reconfigure_callback_; 38 | }; 39 | 40 | } // distance_filter 41 | 42 | #endif // DISTANCE_FILTER_RECONFIGURE_PARAMETER_MANAGER_H 43 | -------------------------------------------------------------------------------- /src/distance_filter/src/distance_filter.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the DistanceFilter class. 3 | */ 4 | 5 | // std. 6 | #include 7 | 8 | // PCL. 9 | #include 10 | 11 | // Our headers. 12 | #include 13 | #include 14 | 15 | 16 | namespace distance_filter { 17 | 18 | DistanceFilter::DistanceFilter(ParameterManager* pm) 19 | : param_manager_ {pm} 20 | { 21 | if (!pm) 22 | throw std::runtime_error {"DistanceFilter: nullptr passed to constructor"}; 23 | } 24 | 25 | 26 | DistanceFilter::PointCloudT::Ptr DistanceFilter::filter( 27 | const PointCloudT::ConstPtr& cloud) 28 | { 29 | pcl::PassThrough passthrough {createPassThroughForCloud(cloud)}; 30 | 31 | PointCloudT::Ptr filtered_cloud {new PointCloudT}; 32 | passthrough.filter(*filtered_cloud); 33 | return filtered_cloud; 34 | } 35 | 36 | pcl::PassThrough 37 | DistanceFilter::createPassThroughForCloud( 38 | const PointCloudT::ConstPtr& cloud) const 39 | { 40 | pcl::PassThrough passthrough; 41 | 42 | passthrough.setInputCloud(cloud); 43 | passthrough.setFilterFieldName("z"); 44 | passthrough.setFilterLimits(param_manager_->minDistance(), 45 | param_manager_->maxDistance()); 46 | 47 | return passthrough; 48 | } 49 | 50 | } // distance_filter 51 | -------------------------------------------------------------------------------- /src/distance_filter/src/parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ParameterManager class. 3 | */ 4 | 5 | // std. 6 | #include // runtime_error 7 | #include // min(), max() 8 | 9 | // Our headers. 10 | #include 11 | 12 | 13 | namespace { 14 | 15 | template 16 | T truncateToRange(T val, T min, T max) 17 | { 18 | return std::min(std::max(val, min), max); 19 | } 20 | 21 | } // anonymous ns 22 | 23 | 24 | namespace distance_filter { 25 | 26 | void ParameterManager::setMinDistance(double val) 27 | { 28 | min_distance_ = truncateToRange(val, distance_lower_limit, 29 | distance_upper_limit); 30 | } 31 | 32 | 33 | void ParameterManager::setMaxDistance(double val) 34 | { 35 | max_distance_ = truncateToRange(val, distance_lower_limit, distance_upper_limit); 36 | } 37 | 38 | 39 | void ParameterManager::setDistances(double min_distance, double max_distance) 40 | { 41 | if (max_distance < min_distance || min_distance < distance_lower_limit || 42 | distance_upper_limit < max_distance) 43 | throw std::runtime_error 44 | {"ParameterManager::setDistances: invalid argument values"}; 45 | 46 | min_distance_ = min_distance; 47 | max_distance_ = max_distance; 48 | } 49 | 50 | } // distance_filter 51 | -------------------------------------------------------------------------------- /src/distance_filter/src/reconfigure_parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ReconfigureParameterManager class. 3 | */ 4 | 5 | // std. 6 | #include // max() 7 | #include // bind(), placeholders 8 | 9 | // Our headers. 10 | #include 11 | 12 | using namespace std; 13 | 14 | 15 | namespace distance_filter { 16 | 17 | ReconfigureParameterManager::ReconfigureParameterManager() 18 | { 19 | reconfigure_callback_ = bind( 20 | &ReconfigureParameterManager::reconfigureCallback, this, 21 | placeholders::_1, placeholders::_2); 22 | reconfigure_server_.setCallback(reconfigure_callback_); 23 | } 24 | 25 | 26 | void ReconfigureParameterManager::reconfigureCallback(ParametersConfig& config, 27 | uint32_t) 28 | { 29 | config.max_distance = max(config.min_distance, config.max_distance); 30 | 31 | ParameterManager::setMinDistance(config.min_distance); 32 | ParameterManager::setMaxDistance(config.max_distance); 33 | } 34 | 35 | } // distance_filter 36 | -------------------------------------------------------------------------------- /src/feature_detector/cfg/Parameters.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "feature_detector" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | surf = gen.add_group("surf") 9 | surf.add("hessian_threshold", double_t, 0, \ 10 | "Only local extrema with a Hessian-determinant above this will " + \ 11 | "be kept as keypoints", \ 12 | 500., 2., 10000.) 13 | surf.add("num_octaves", int_t, 0, \ 14 | "Number of pyramid octaves", 15 | 4, 1, 20) 16 | surf.add("num_octave_layers", int_t, 0, \ 17 | "Number of images in each octave", 18 | 3, 1, 20) 19 | surf.add("extended", bool_t, 0, \ 20 | "false: 64-element; true: 128-element descriptor", 21 | False) 22 | surf.add("upright", bool_t, 0, \ 23 | "true: don't compute orientation", 24 | False) 25 | 26 | matcher = gen.add_group("matcher") 27 | norm_types = gen.enum([ \ 28 | gen.const("L1", int_t, 2, "Sum of absolute differences"), \ 29 | gen.const("L2", int_t, 4, "Sqrt of sum of squared differences")], \ 30 | "An enum to set the norm-type, used by the matcher") 31 | # The constants have to be identical to the cv::NORM_* consts. 32 | matcher.add("norm_type", int_t, 0, "Type for calculating norms", 4, 2, 4, \ 33 | edit_method=norm_types) 34 | matcher.add("cross_check", bool_t, 0, "Cross-checking", False) 35 | 36 | target_selection = gen.add_group("target_selection") 37 | target_selection.add("filename", str_t, 0, \ 38 | "Name of file for saving/loading target selection", "") 39 | target_selection.add("save_selection", bool_t, 0, \ 40 | "Save current area/target selection to file", False) 41 | target_selection.add("load_selection", bool_t, 0, \ 42 | "Load area/target selection from file", False) 43 | 44 | gen.add("match_distance_threshold", double_t, 0, \ 45 | "Only matches above this threshold are considered", \ 46 | 0.25, 0.1, 100.) 47 | gen.add("ransac_reproj_threshold", double_t, 0, \ 48 | "Only points with a smaller reprojection-error are considered " + \ 49 | "when finding the homography", \ 50 | 3.0, 0.5, 100.) 51 | gen.add("object_name", str_t, 0, \ 52 | "Name that is given to detected objects", "") 53 | 54 | exit(gen.generate(PACKAGE, "feature_detector", "Parameters")) 55 | -------------------------------------------------------------------------------- /src/feature_detector/cfg/ParametersOld.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "feature_detector_node" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # surf = gen.add_group("surf") # does not work yet 9 | gen.add("hessian_threshold", double_t, 0, \ 10 | "Only local extrema with a Hessian-determinant above this will " + \ 11 | "be kept as keypoints", \ 12 | 500., 2., 10000.) 13 | gen.add("num_octaves", int_t, 0, \ 14 | "Number of pyramid octaves", 15 | 4, 1, 20) 16 | gen.add("num_octave_layers", int_t, 0, \ 17 | "Number of images in each octave", 18 | 3, 1, 20) 19 | gen.add("extended", bool_t, 0, \ 20 | "false: 64-element; true: 128-element descriptor", 21 | False) 22 | gen.add("upright", bool_t, 0, \ 23 | "true: don't compute orientation", 24 | False) 25 | 26 | # matcher = gen.add_group("matcher") 27 | norm_types = gen.enum([ \ 28 | gen.const("L1", int_t, 2, "Sum of absolute differences"), \ 29 | gen.const("L2", int_t, 4, "Sqrt of sum of squared differences")], \ 30 | "An enum to set the norm-type, used by the matcher") 31 | # The constants have to be identical to the cv::NORM_* consts. 32 | gen.add("norm_type", int_t, 0, "Type for calculating norms", 4, 2, 4, \ 33 | edit_method=norm_types) 34 | gen.add("cross_check", bool_t, 0, "Cross-checking", False) 35 | 36 | # target_selection = gen.add_group("target_selection") 37 | gen.add("filename", str_t, 0, \ 38 | "Name of file for saving/loading target selection", "") 39 | gen.add("save_selection", bool_t, 0, \ 40 | "Save current area/target selection to file", False) 41 | gen.add("load_selection", bool_t, 0, \ 42 | "Load area/target selection from file", False) 43 | 44 | gen.add("match_distance_threshold", double_t, 0, \ 45 | "Only matches above this threshold are considered", \ 46 | 0.25, 0.1, 100.) 47 | gen.add("ransac_reproj_threshold", double_t, 0, \ 48 | "Only points with a smaller reprojection-error are considered " + \ 49 | "when finding the homography", \ 50 | 3.0, 0.5, 100.) 51 | gen.add("object_name", str_t, 0, \ 52 | "Name that is given to detected objects", "") 53 | 54 | exit(gen.generate(PACKAGE, "feature_detector", "Parameters")) 55 | -------------------------------------------------------------------------------- /src/feature_detector/include/feature_detector/descriptor_matcher_groupbox.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the DescriptorMatcherGroupBox 3 | */ 4 | 5 | #ifndef FEATURE_DETECTOR_DESCRIPTOR_MATCHER_GROUPBOX_H 6 | #define FEATURE_DETECTOR_DESCRIPTOR_MATCHER_GROUPBOX_H 7 | 8 | // std. 9 | #include 10 | 11 | // Qt. 12 | #include 13 | #include 14 | 15 | // Headers of this package. 16 | #include "observed_parameter_manager.h" 17 | 18 | // Forward declarations. 19 | class QWidget; 20 | class QLabel; 21 | class QComboBox; 22 | class QCheckBox; 23 | class QGridLayout; 24 | 25 | 26 | namespace feature_detector { 27 | 28 | /** \brief QGroupBox for accessing the parameters of a FeatureDetector that are 29 | * related to the descriptor-matching process. 30 | */ 31 | class DescriptorMatcherGroupBox : public QGroupBox { 32 | Q_OBJECT 33 | 34 | public: 35 | /** \brief Creates and initializes the GroupBox. 36 | * 37 | * \param[in] parent Pointer to QWidget that serves as a parent of the 38 | * GroupBox. 39 | */ 40 | explicit DescriptorMatcherGroupBox(QWidget* parent = nullptr); 41 | 42 | /** \brief Updates the view of the GroupBox so that it displays the current 43 | * values of the parameters. 44 | */ 45 | void update() const; 46 | 47 | /** \brief Sets the parameter-manager of the FeatureDetector. 48 | * 49 | * The ObservedParameterManager manages the parameters of a FeatureDetector 50 | * object. 51 | * The manager represents the Model of the MVC-pattern, while the GroupBox 52 | * represents the View and the Controller. 53 | */ 54 | void setParameterManager(ObservedParameterManager*); 55 | 56 | private slots: 57 | void requestNormTypeChange(const QString&) const; 58 | void requestCrossCheckChange(bool) const; 59 | 60 | private: 61 | // Initialization. 62 | void initNormTypeWidgets(); 63 | void initCrossCheckCheckBox(); 64 | 65 | QGridLayout* createMainLayout() const; 66 | 67 | // Static consts. 68 | static const std::map norm_type_mapping; 69 | 70 | // Data members. 71 | ObservedParameterManager* param_manager_ {nullptr}; 72 | 73 | QLabel* norm_type_label_; 74 | QComboBox* norm_type_combo_; 75 | QCheckBox* cross_check_checkbox_; 76 | }; 77 | 78 | } // feature_detector 79 | 80 | #endif // FEATURE_DETECTOR_DESCRIPTOR_MATCHER_GROUPBOX_H 81 | -------------------------------------------------------------------------------- /src/feature_detector/include/feature_detector/groupbox.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the GroupBox class. 3 | */ 4 | 5 | #ifndef FEATURE_DETECTOR_GROUPBOX_H 6 | #define FEATURE_DETECTOR_GROUPBOX_H 7 | 8 | // TODO: Add widget for object-name. 9 | 10 | // Qt. 11 | #include 12 | 13 | // Headers of this project. 14 | #include 15 | 16 | // Forward declarations. 17 | class QWidget; 18 | class QVBoxLayout; 19 | 20 | namespace feature_detector { 21 | 22 | class ObservedParameterManager; 23 | class SurfGroupBox; 24 | class DescriptorMatcherGroupBox; 25 | class ThresholdsGroupBox; 26 | 27 | 28 | /** \brief QGroupBox for accessing the parameters of a FeatureDetector object. 29 | */ 30 | class GroupBox : public QGroupBox, public object_detection::Observer { 31 | Q_OBJECT 32 | 33 | public: 34 | /** \brief Creates and initializes the GroupBox. 35 | * 36 | * \param[in] parent Pointer to QWidget that serves as a parent of the 37 | * GroupBox. 38 | */ 39 | explicit GroupBox(QWidget* parent = nullptr); 40 | 41 | /** \brief Updates the view of the GroupBox so that it displays the current 42 | * values of the parameters. 43 | */ 44 | void update() override; 45 | 46 | /** \brief Sets the parameter-manager of the FeatureDetector. 47 | * 48 | * The ObservedParameterManager manages the parameters of a FeatureDetector 49 | * object. 50 | * The manager represents the Model of the MVC-pattern, while the GroupBox 51 | * represents the View and the Controller. 52 | */ 53 | void setParameterManager(ObservedParameterManager*); 54 | 55 | private: 56 | QVBoxLayout* createMainLayout() const; 57 | 58 | // Data members. 59 | SurfGroupBox* surf_groupbox_; 60 | DescriptorMatcherGroupBox* descriptor_matcher_groupbox_; 61 | ThresholdsGroupBox* thresholds_groupbox_; 62 | }; 63 | 64 | } // feature_detector 65 | 66 | #endif // FEATURE_DETECTOR_GROUPBOX_H 67 | -------------------------------------------------------------------------------- /src/feature_detector/include/feature_detector/observed_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ObservedParameterManager class. 3 | */ 4 | 5 | #ifndef FEATURE_DETECTOR_OBSERVED_PARAMETER_MANAGER_H 6 | #define FEATURE_DETECTOR_OBSERVED_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include 10 | 11 | // Headers of this project. 12 | #include 13 | #include 14 | #include "parameter_manager.h" 15 | 16 | 17 | namespace cv { 18 | class FileStorage; 19 | } 20 | 21 | namespace feature_detector { 22 | 23 | /** \brief Observable parameter-manager for a FeatureDetector object. 24 | * 25 | * This ParameterManager inherits from object_detection:Subject, 26 | * making it possible to register observers for it. 27 | * It also inherits from object_detection_2d::Parametrizable. 28 | * 29 | * Using this parameter-manager makes it possible to access the parameters 30 | * of a FeatureDetector via the object_detection_2d::GUI. 31 | */ 32 | class ObservedParameterManager : public ParameterManager, 33 | public object_detection::Subject, 34 | public object_detection_2d::Parametrizable { 35 | public: 36 | // 37 | // Parametrizable interface. 38 | // 39 | 40 | /** \brief Writes all parameters to the given FileStorage object. */ 41 | void writeParametersToStorage(cv::FileStorage&) const override; 42 | 43 | /** \brief Reads all parameters from the given FileStorage object 44 | * and sets its parameters accordingly. 45 | */ 46 | void setParametersFromStorage(cv::FileStorage&) override; 47 | 48 | // 49 | // Setters. 50 | // 51 | 52 | /** \brief Sets the name of detected objects. */ 53 | void setObjectName(const std::string&); 54 | /** \brief Sets the hessian-threshold parameter. */ 55 | void setHessianThreshold(double); 56 | /** \brief Sets the number-octaves parameter. */ 57 | void setNumOctaves(int); 58 | /** \brief Sets the number-octave-layers parameter. */ 59 | void setNumOctaveLayers(int); 60 | /** \brief Sets the extended parameter. */ 61 | void setExtended(bool); 62 | /** \brief Sets the upright parameter. */ 63 | void setUpright(bool); 64 | /** \brief Sets the norm-type parameter. */ 65 | void setNormType(NormType); 66 | /** \brief Sets the cross-check parameter. */ 67 | void setCrossCheck(bool); 68 | /** \brief Sets the match-distance parameter. */ 69 | void setMatchDistanceThreshold(double); 70 | /** \brief Sets the ransac-reprojection-threhold parameter. */ 71 | void setRansacReprojThreshold(double); 72 | }; 73 | 74 | } // feature_detector 75 | 76 | #endif // FEATURE_DETECTOR_OBSERVED_PARAMETER_MANAGER_H 77 | -------------------------------------------------------------------------------- /src/feature_detector/include/feature_detector/reconfigure_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ReconfigureParameterManager class. 3 | */ 4 | 5 | #ifndef FEATURE_DETECTOR_RECONFIGURE_PARAMETER_MANAGER_H 6 | #define FEATURE_DETECTOR_RECONFIGURE_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include 10 | 11 | // dynamic_reconfigure. 12 | #include 13 | 14 | // Headers of this package. 15 | #include 16 | #include "parameter_manager.h" 17 | 18 | 19 | namespace feature_detector { 20 | 21 | /** \brief dynamic_reconfigure parameter-server for FeatureDetector objects. 22 | * 23 | * A ReconfigureParameterManager object allows to manage the parameters 24 | * of a FeatureDetector object via a corresponding set of dynamic_reconfigure 25 | * parameters. 26 | */ 27 | class ReconfigureParameterManager : public ParameterManager { 28 | public: 29 | /** \brief Initializes the dynamic_reconfigure server and registers a callback 30 | * for this server. 31 | */ 32 | ReconfigureParameterManager(); 33 | 34 | private: 35 | void reconfigureCallback(feature_detector::ParametersConfig& config, 36 | uint32_t); 37 | 38 | void saveSelection(const std::string& filename) const; 39 | void loadSelection(const std::string& filename); 40 | // or are those part of ParameterManager? 41 | 42 | // dynamic_reconfigure server & callback. 43 | dynamic_reconfigure::Server 44 | reconfigure_server_; 45 | dynamic_reconfigure::Server:: 46 | CallbackType reconfigure_callback_; 47 | }; 48 | 49 | } // feature_detector 50 | 51 | #endif // FEATURE_DETECTOR_RECONFIGURE_PARAMETER_MANAGER_H 52 | -------------------------------------------------------------------------------- /src/feature_detector/include/feature_detector/surf_groupbox.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the SurfGroupBox class. 3 | */ 4 | 5 | #ifndef FEATURE_DETECTOR_SURF_GROUPBOX_H 6 | #define FEATURE_DETECTOR_SURF_GROUPBOX_H 7 | 8 | // Qt. 9 | #include 10 | 11 | // Forward declarations. 12 | class QWidget; 13 | class QLabel; 14 | class QSpinBox; 15 | class QDoubleSpinBox; 16 | class QCheckBox; 17 | class QGridLayout; 18 | 19 | 20 | namespace feature_detector { 21 | 22 | class ObservedParameterManager; 23 | 24 | 25 | /** \brief QGroupBox for accessing the parameters of a FeatureDetector that are 26 | * related to the computation of SURF features. 27 | */ 28 | class SurfGroupBox : public QGroupBox { 29 | Q_OBJECT 30 | 31 | public: 32 | /** \brief Creates and initializes the GroupBox. 33 | * 34 | * \param[in] parent Pointer to QWidget that serves as a parent of the 35 | * GroupBox. 36 | */ 37 | explicit SurfGroupBox(QWidget* parent = nullptr); 38 | 39 | /** \brief Updates the view of the GroupBox so that it displays the current 40 | * values of the parameters. 41 | */ 42 | void update() const; 43 | 44 | /** \brief Sets the parameter-manager of the FeatureDetector. 45 | * 46 | * The ObservedParameterManager manages the parameters of a FeatureDetector 47 | * object. 48 | * The manager represents the Model of the MVC-pattern, while the GroupBox 49 | * represents the View and the Controller. 50 | */ 51 | void setParameterManager(ObservedParameterManager*); 52 | 53 | private slots: 54 | void requestHessianThresholdChange(double) const; 55 | void requestNumOctavesChange(int) const; 56 | void requestNumOctaveLayersChange(int) const; 57 | void requestExtendedChange(bool) const; 58 | void requestUprightChange(bool) const; 59 | 60 | private: 61 | // Initialization. 62 | void initHessianThresholdWidgets(); 63 | void initNumOctavesWidgets(); 64 | void initNumOctaveLayersWidgets(); 65 | void initExtendedCheckBox(); 66 | void initUprightCheckBox(); 67 | 68 | QGridLayout* createMainLayout() const; 69 | 70 | // Data members. 71 | ObservedParameterManager* param_manager_ {nullptr}; 72 | 73 | QLabel* hessian_thresh_label_; 74 | QDoubleSpinBox* hessian_thresh_spinbox_; 75 | QLabel* num_octaves_label_; 76 | QSpinBox* num_octaves_spinbox_; 77 | QLabel* num_octave_layers_label_; 78 | QSpinBox* num_octave_layers_spinbox_; 79 | QCheckBox* extended_checkbox_; 80 | QCheckBox* upright_checkbox_; 81 | }; 82 | 83 | } // feature_detector 84 | 85 | #endif // FEATURE_DETECTOR_SURF_GROUPBOX_H 86 | -------------------------------------------------------------------------------- /src/feature_detector/include/feature_detector/thresholds_groupbox.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ThresholdsGroupBox class. 3 | */ 4 | 5 | #ifndef FEATURE_DETECTOR_THRESHOLDS_GROUPBOX_H 6 | #define FEATURE_DETECTOR_THRESHOLDS_GROUPBOX_H 7 | 8 | // Qt. 9 | #include 10 | 11 | // Forward declarations. 12 | class QWidget; 13 | class QLabel; 14 | class QDoubleSpinBox; 15 | class QGridLayout; 16 | 17 | 18 | namespace feature_detector { 19 | 20 | class ObservedParameterManager; 21 | 22 | 23 | /** \brief QGroupBox for accessing the parameters of a FeatureDetector that are 24 | * thresholds that are unrelated to the feature-detector or the matcher. 25 | */ 26 | class ThresholdsGroupBox : public QGroupBox { 27 | Q_OBJECT 28 | 29 | public: 30 | /** \brief Creates and initializes the GroupBox. 31 | * 32 | * \param[in] parent Pointer to QWidget that serves as a parent of the 33 | * GroupBox. 34 | */ 35 | explicit ThresholdsGroupBox(QWidget* parent = nullptr); 36 | 37 | /** \brief Updates the view of the GroupBox so that it displays the current 38 | * values of the parameters. 39 | */ 40 | void update() const; 41 | 42 | /** \brief Sets the parameter-manager of the FeatureDetector. 43 | * 44 | * The ObservedParameterManager manages the parameters of a FeatureDetector 45 | * object. 46 | * The manager represents the Model of the MVC-pattern, while the GroupBox 47 | * represents the View and the Controller. 48 | */ 49 | void setParameterManager(ObservedParameterManager*); 50 | 51 | private slots: 52 | // void requestKptResponseChange(double) const; 53 | void requestMatchDistanceChange(double) const; 54 | void requestRansacReprojChange(double) const; 55 | 56 | private: 57 | // Initialization. 58 | void initMatchDistanceWidgets(); 59 | void initRansacReprojWidgets(); 60 | 61 | QGridLayout* createMainLayout() const; 62 | 63 | // Data members. 64 | ObservedParameterManager* param_manager_ {nullptr}; 65 | 66 | QLabel* match_distance_label_; 67 | QDoubleSpinBox* match_distance_spinbox_; 68 | 69 | QLabel* ransac_reproj_label_; 70 | QDoubleSpinBox* ransac_reproj_spinbox_; 71 | }; 72 | 73 | } // feature_detector 74 | 75 | #endif // FEATURE_DETECTOR_THRESHOLDS_GROUPBOX_H 76 | -------------------------------------------------------------------------------- /src/feature_detector/src/groupbox.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the GroupBox class. 3 | */ 4 | 5 | // Qt. 6 | #include 7 | #include 8 | #include 9 | 10 | // Headers of this package. 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | namespace feature_detector { 19 | 20 | GroupBox::GroupBox(QWidget* parent) 21 | : QGroupBox {"Feature Detector", parent}, 22 | surf_groupbox_ {new SurfGroupBox{this}}, 23 | descriptor_matcher_groupbox_ { new DescriptorMatcherGroupBox{this}}, 24 | thresholds_groupbox_ {new ThresholdsGroupBox{this}} 25 | { 26 | QVBoxLayout* main_layout {createMainLayout()}; 27 | setLayout(main_layout); 28 | } 29 | 30 | 31 | QVBoxLayout* GroupBox::createMainLayout() const 32 | { 33 | QVBoxLayout* layout {new QVBoxLayout}; 34 | layout->addWidget(surf_groupbox_); 35 | layout->addWidget(descriptor_matcher_groupbox_); 36 | layout->addWidget(thresholds_groupbox_); 37 | return layout; 38 | } 39 | 40 | 41 | void GroupBox::update() 42 | { 43 | surf_groupbox_->update(); 44 | descriptor_matcher_groupbox_->update(); 45 | thresholds_groupbox_->update(); 46 | } 47 | 48 | 49 | void GroupBox::setParameterManager(ObservedParameterManager* pm) 50 | { 51 | surf_groupbox_->setParameterManager(pm); 52 | descriptor_matcher_groupbox_->setParameterManager(pm); 53 | thresholds_groupbox_->setParameterManager(pm); 54 | } 55 | 56 | } // feature_detector 57 | -------------------------------------------------------------------------------- /src/hsv_filter/cfg/Parameters.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "hsv_filter" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | H_LOWER_LIMIT = 0 9 | H_UPPER_LIMIT = 179 10 | gen.add("h_min", int_t, 0, \ 11 | "Areas with lower h-values are filtered out", \ 12 | H_LOWER_LIMIT, H_LOWER_LIMIT, H_UPPER_LIMIT) 13 | gen.add("h_max", int_t, 0, \ 14 | "Areas with lower h-values are filtered out", \ 15 | H_UPPER_LIMIT, H_LOWER_LIMIT, H_UPPER_LIMIT) 16 | 17 | S_LOWER_LIMIT = 0 18 | S_UPPER_LIMIT = 255 19 | gen.add("s_min", int_t, 0, \ 20 | "Areas with lower s-values are filtered out", \ 21 | S_LOWER_LIMIT, S_LOWER_LIMIT, S_UPPER_LIMIT) 22 | gen.add("s_max", int_t, 0, \ 23 | "Areas with lower s-values are filtered out", \ 24 | S_UPPER_LIMIT, S_LOWER_LIMIT, S_UPPER_LIMIT) 25 | 26 | V_LOWER_LIMIT = 0 27 | V_UPPER_LIMIT = 255 28 | gen.add("v_min", int_t, 0, \ 29 | "Areas with lower v-values are filtered out", \ 30 | V_LOWER_LIMIT, V_LOWER_LIMIT, V_UPPER_LIMIT) 31 | gen.add("v_max", int_t, 0, \ 32 | "Areas with lower v-values are filtered out", \ 33 | V_UPPER_LIMIT, V_LOWER_LIMIT, V_UPPER_LIMIT) 34 | 35 | exit(gen.generate(PACKAGE, "hsv_filter", "Parameters")) 36 | -------------------------------------------------------------------------------- /src/hsv_filter/include/hsv_filter/hsv_filter.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the HSVFilter class. 3 | */ 4 | 5 | #ifndef HSV_FILTER_HSV_FILTER_H 6 | #define HSV_FILTER_HSV_FILTER_H 7 | 8 | // ROS & OpenCV headers. 9 | #include // cv::Mat 10 | 11 | // object-detection headers. 12 | #include 13 | 14 | 15 | namespace hsv_filter { 16 | 17 | class ParameterManager; 18 | 19 | /** \brief Filter for filtering images according to their HSV pixel-values. */ 20 | class HSVFilter : public object_detection_2d::Filter { 21 | public: 22 | /** \brief Creates an HSVFilter and initializes its parameter-manager. 23 | * 24 | * \param[in] pm Pointer to a valid ParameterManager. 25 | */ 26 | explicit HSVFilter(ParameterManager* pm); 27 | 28 | /** \brief Filters the given image. 29 | * 30 | * Only pixels whose Hue, Saturation and Value values lie inside 31 | * adjustable ranges pass through. 32 | * 33 | * \param[in] bgr_image Image that is filtered. 34 | * \return New image, containing the filtered image. 35 | */ 36 | cv::Mat filter(const cv::Mat& bgr_image) override; 37 | 38 | private: 39 | cv::Mat threshold(const cv::Mat& hsv_image) const; 40 | 41 | // Data members. 42 | ParameterManager* param_manager_; 43 | }; 44 | 45 | } // hsv_filter 46 | 47 | #endif // HSV_FILTER_HSV_FILTER_H 48 | -------------------------------------------------------------------------------- /src/hsv_filter/include/hsv_filter/observed_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ObservedParameterManager class. 3 | */ 4 | 5 | #ifndef HSV_FILTER_OBSERVED_PARAMETER_MANAGER_H 6 | #define HSV_FILTER_OBSERVED_PARAMETER_MANAGER_H 7 | 8 | // Headers of this project. 9 | #include 10 | #include 11 | #include "parameter_manager.h" 12 | 13 | 14 | namespace cv { 15 | class FileStorage; 16 | } 17 | 18 | namespace hsv_filter { 19 | 20 | /** \brief Observable parameter-manager for an HSVFilter object. 21 | * 22 | * This ParameterManager inherits from object_detection:Subject, 23 | * making it possible to register observers for it. 24 | * It also inherits from object_detection_2d::Parametrizable. 25 | * 26 | * Using this parameter-manager makes it possible to access the parameters 27 | * of an HSVFilter via the object_detection_2d::GUI. 28 | */ 29 | class ObservedParameterManager : public ParameterManager, 30 | public object_detection::Subject, 31 | public object_detection_2d::Parametrizable { 32 | public: 33 | // 34 | // Parametrizable interface. 35 | // 36 | 37 | /** \brief Writes all parameters to the given FileStorage object. */ 38 | void writeParametersToStorage(cv::FileStorage&) const override; 39 | 40 | /** \brief Reads all parameters from the given FileStorage object 41 | * and sets its parameters accordingly. 42 | */ 43 | void setParametersFromStorage(cv::FileStorage&) override; 44 | 45 | // 46 | // Setters. 47 | // 48 | /** \brief Sets the h-min parameter. */ 49 | void setHMin(int); 50 | /** \brief Sets the h-min parameter. */ 51 | void setHMax(int); 52 | 53 | /** \brief Sets the s-min parameter. */ 54 | void setSMin(int); 55 | /** \brief Sets the s-min parameter. */ 56 | void setSMax(int); 57 | 58 | /** \brief Sets the v-min parameter. */ 59 | void setVMin(int); 60 | /** \brief Sets the v-min parameter. */ 61 | void setVMax(int); 62 | }; 63 | 64 | } // hsv_filter 65 | 66 | #endif // HSV_FILTER_OBSERVED_PARAMETER_MANAGER_H 67 | -------------------------------------------------------------------------------- /src/hsv_filter/include/hsv_filter/parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ParameterManager class. 3 | */ 4 | 5 | /* TODO: Maybe it's better to use delegation instead of inheritance. 6 | * A way to do this would be to give the HSVFilter a ParameterManager as a 7 | * member (not just a reference) and to give the ParameterManager object 8 | * a member that is the only one that is allowed to set the parameters, 9 | * therefore encapsulating how the parameters are set (from a reconfigure- 10 | * callback or through some request of an observer). 11 | * The HSVFilter would than provide a method for setting this setter-object 12 | * in the filter's parameter-manager object. 13 | */ 14 | 15 | #ifndef HSV_FILTER_PARAMETER_MANAGER_H 16 | #define HSV_FILTER_PARAMETER_MANAGER_H 17 | 18 | 19 | namespace hsv_filter { 20 | 21 | /** \brief Parameter-manager for BinaryDetector objects. 22 | * 23 | * A ParameterManager object manages the parameters of 24 | * a BinaryDetector object and provides getter- and setter-methods for 25 | * accessing those parameters. 26 | */ 27 | class ParameterManager { 28 | public: 29 | // 30 | // Limits / Consts. 31 | // 32 | /** \brief Lower limit of the h-min and h-max parameters. */ 33 | static constexpr int h_lower_limit {0}; 34 | /** \brief Upper limit of the h-min and h-max parameters. */ 35 | static constexpr int h_upper_limit {179}; 36 | 37 | /** \brief Lower limit of the s-min and s-max parameters. */ 38 | static constexpr int s_lower_limit {0}; 39 | /** \brief Upper limit of the s-min and s-max parameters. */ 40 | static constexpr int s_upper_limit {255}; 41 | 42 | /** \brief Lower limit of the v-min and v-max parameters. */ 43 | static constexpr int v_lower_limit {0}; 44 | /** \brief Upper limit of the v-min and v-max parameters. */ 45 | static constexpr int v_upper_limit {255}; 46 | 47 | // 48 | // Getters. 49 | // 50 | int hMin() const { return h_min_; } 51 | int hMax() const { return h_max_; } 52 | 53 | int sMin() const { return s_min_; } 54 | int sMax() const { return s_max_; } 55 | 56 | int vMin() const { return v_min_; } 57 | int vMax() const { return v_max_; } 58 | 59 | // 60 | // Setters. Maybe make these protected. 61 | // 62 | void setHMin(int val); 63 | void setHMax(int val); 64 | 65 | void setSMin(int val); 66 | void setSMax(int val); 67 | 68 | void setVMin(int val); 69 | void setVMax(int val); 70 | 71 | private: 72 | // Parameters. 73 | int h_min_ {h_lower_limit}; 74 | int h_max_ {h_upper_limit}; 75 | 76 | int s_min_ {s_lower_limit}; 77 | int s_max_ {s_upper_limit}; 78 | 79 | int v_min_ {v_lower_limit}; 80 | int v_max_ {v_upper_limit}; 81 | }; 82 | 83 | } // hsv_filter 84 | 85 | #endif // HSV_FILTER_PARAMETER_MANAGER_H 86 | -------------------------------------------------------------------------------- /src/hsv_filter/include/hsv_filter/reconfigure_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ReconfigureParameterManager class. 3 | */ 4 | 5 | #ifndef HSV_FILTER_RECONFIGURE_PARAMETER_MANAGER_H 6 | #define HSV_FILTER_RECONFIGURE_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include // uint32_t 10 | 11 | // dynamic_reconfigure. 12 | #include 13 | 14 | // Headers of this package. 15 | #include 16 | #include 17 | 18 | 19 | namespace hsv_filter { 20 | 21 | /** \brief dynamic_reconfigure parameter-manager for HSVFilter objects. 22 | * 23 | * A ReconfigureParameterManager object allows to manage the parameters 24 | * of a HSVFilter object via a corresponding set of dynamic_reconfigure 25 | * parameters. 26 | */ 27 | class ReconfigureParameterManager : public ParameterManager { 28 | public: 29 | /** \brief Initializes the dynamic_reconfigure server and registers a callback 30 | * for this server. 31 | */ 32 | ReconfigureParameterManager(); 33 | 34 | private: 35 | void reconfigureCallback(hsv_filter::ParametersConfig& config, 36 | uint32_t); 37 | 38 | // Dynamic reconfigure server & callback. 39 | dynamic_reconfigure::Server::CallbackType 40 | reconfigure_callback_; 41 | dynamic_reconfigure::Server 42 | reconfigure_server_; 43 | }; 44 | 45 | } // hsv_filter 46 | 47 | #endif // HSV_FILTER_RECONFIGURE_PARAMETER_MANAGER_H 48 | -------------------------------------------------------------------------------- /src/hsv_filter/launch/hsv_filter.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/hsv_filter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hsv_filter 4 | 0.0.0 5 | The hsv_filter package 6 | 7 | 8 | 9 | 10 | janosch 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | dynamic_reconfigure 45 | object_detection_2d 46 | object_detection 47 | cv_bridge 48 | 49 | dynamic_reconfigure 50 | object_detection_2d 51 | object_detection 52 | cv_bridge 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /src/hsv_filter/params/body_milk.yaml: -------------------------------------------------------------------------------- 1 | !!python/object/new:dynamic_reconfigure.encoding.Config 2 | dictitems: 3 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 4 | dictitems: 5 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 6 | state: [] 7 | h_max: 138 8 | h_min: 99 9 | id: 0 10 | name: Default 11 | parameters: !!python/object/new:dynamic_reconfigure.encoding.Config 12 | state: [] 13 | parent: 0 14 | s_max: 255 15 | s_min: 157 16 | state: true 17 | type: '' 18 | v_max: 218 19 | v_min: 0 20 | state: [] 21 | h_max: 138 22 | h_min: 99 23 | s_max: 255 24 | s_min: 157 25 | v_max: 218 26 | v_min: 0 27 | state: [] 28 | -------------------------------------------------------------------------------- /src/hsv_filter/params/book.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | HSVFilter-h_min: 134 4 | HSVFilter-h_max: 179 5 | HSVFilter-s_min: 109 6 | HSVFilter-s_max: 255 7 | HSVFilter-v_min: 0 8 | HSVFilter-v_max: 255 9 | MorphologyFilter-operation: 2 10 | MorphologyFilter-num_iterations: 2 11 | BinaryDetector-min_length_frac: 1.7272000000000001e-01 12 | BinaryDetector-max_length_frac: 1.3377749999999999e+00 13 | -------------------------------------------------------------------------------- /src/hsv_filter/params/pink_pad.yaml: -------------------------------------------------------------------------------- 1 | !!python/object/new:dynamic_reconfigure.encoding.Config 2 | dictitems: 3 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 4 | dictitems: 5 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 6 | state: [] 7 | h_max: 179 8 | h_min: 122 9 | id: 0 10 | name: Default 11 | parameters: !!python/object/new:dynamic_reconfigure.encoding.Config 12 | state: [] 13 | parent: 0 14 | s_max: 237 15 | s_min: 41 16 | state: true 17 | type: '' 18 | v_max: 255 19 | v_min: 152 20 | state: [] 21 | h_max: 179 22 | h_min: 122 23 | s_max: 237 24 | s_min: 41 25 | v_max: 255 26 | v_min: 152 27 | state: [] 28 | -------------------------------------------------------------------------------- /src/hsv_filter/src/hsv_filter.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the HSVFilter class. 3 | */ 4 | // std. 5 | #include // runtime_error 6 | 7 | // ROS & OpenCV. 8 | #include // Mat, Scalar, inRange() 9 | #include // COLOR_*, cvtColor(), 10 | 11 | // Our headers. 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | 17 | 18 | namespace { 19 | 20 | cv::Mat bgr2hsv(const cv::Mat& bgr_image) 21 | { 22 | cv::Mat hsv_image; 23 | cv::cvtColor(bgr_image, hsv_image, cv::COLOR_BGR2HSV); 24 | return hsv_image; 25 | } 26 | 27 | 28 | cv::Mat gray2bgr(const cv::Mat& gray_image) 29 | { 30 | cv::Mat bgr_image; 31 | cv::cvtColor(gray_image, bgr_image, cv::COLOR_GRAY2BGR); 32 | return bgr_image; 33 | } 34 | 35 | } // anonymous namespace 36 | 37 | 38 | namespace hsv_filter { 39 | 40 | HSVFilter::HSVFilter(ParameterManager* pm) 41 | : param_manager_{pm} 42 | { 43 | if (!pm) 44 | throw runtime_error {"HSVFilter: nullptr given to constructor"}; 45 | } 46 | 47 | 48 | cv::Mat HSVFilter::filter(const cv::Mat& bgr_image) 49 | { 50 | cv::Mat hsv_image {bgr2hsv(bgr_image)}; 51 | cv::Mat thresholded_image {threshold(hsv_image)}; 52 | return gray2bgr(thresholded_image); 53 | } 54 | 55 | 56 | cv::Mat HSVFilter::threshold(const cv::Mat& hsv_image) const 57 | { 58 | auto& pm {param_manager_}; // shorthand for convenience 59 | 60 | cv::Mat thresholded_image; 61 | cv::inRange(hsv_image, 62 | cv::Scalar(pm->hMin(), pm->sMin(), pm->vMin()), 63 | cv::Scalar(pm->hMax(), pm->sMax(), pm->vMax()), 64 | thresholded_image); 65 | return thresholded_image; 66 | } 67 | 68 | } // hsv_filter 69 | -------------------------------------------------------------------------------- /src/hsv_filter/src/parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ParameterManager class. 3 | */ 4 | 5 | // std. 6 | #include // min(), max() 7 | 8 | // Headers of this project. 9 | #include 10 | 11 | 12 | namespace { 13 | 14 | inline int truncateToRange(int val, int min, int max) 15 | { 16 | return std::min(std::max(val, min), max); 17 | } 18 | 19 | } // anonymous ns 20 | 21 | 22 | namespace hsv_filter { 23 | 24 | void ParameterManager::setHMin(int val) 25 | { 26 | h_min_ = truncateToRange(val, h_lower_limit, h_upper_limit); 27 | } 28 | 29 | void ParameterManager::setHMax(int val) 30 | { 31 | h_max_ = truncateToRange(val, h_lower_limit, h_upper_limit); 32 | } 33 | 34 | void ParameterManager::setSMin(int val) 35 | { 36 | s_min_ = truncateToRange(val, s_lower_limit, s_upper_limit); 37 | } 38 | 39 | void ParameterManager::setSMax(int val) 40 | { 41 | s_max_ = truncateToRange(val, s_lower_limit, s_upper_limit); 42 | } 43 | 44 | void ParameterManager::setVMin(int val) 45 | { 46 | v_min_ = truncateToRange(val, v_lower_limit, v_upper_limit); 47 | } 48 | 49 | void ParameterManager::setVMax(int val) 50 | { 51 | v_max_ = truncateToRange(val, v_lower_limit, v_upper_limit); 52 | } 53 | 54 | } // hsv_filter 55 | -------------------------------------------------------------------------------- /src/hsv_filter/src/reconfigure_parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ReconfigureParameterManager class. 3 | */ 4 | 5 | // std. 6 | #include // bind(), placeholders 7 | 8 | // Headers of this project. 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | 15 | namespace hsv_filter { 16 | 17 | ReconfigureParameterManager::ReconfigureParameterManager() 18 | : reconfigure_callback_{bind( 19 | &ReconfigureParameterManager::reconfigureCallback, this, 20 | placeholders::_1, placeholders::_2)} 21 | { 22 | reconfigure_server_.setCallback(reconfigure_callback_); 23 | } 24 | 25 | 26 | void ReconfigureParameterManager::reconfigureCallback( 27 | hsv_filter::ParametersConfig& config, uint32_t) 28 | { 29 | setHMin(config.h_min); 30 | setHMax(config.h_max); 31 | 32 | setSMin(config.s_min); 33 | setSMax(config.s_max); 34 | 35 | setVMin(config.v_min); 36 | setVMax(config.v_max); 37 | } 38 | 39 | } // hsv_filter 40 | -------------------------------------------------------------------------------- /src/morphology_filter/cfg/Parameters.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "morphology_filter" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | operations = gen.enum([ \ 9 | gen.const("erode", int_t, 0, "erode"), \ 10 | gen.const("dilate", int_t, 1, "dilate"), \ 11 | gen.const("open", int_t, 2, "open"), \ 12 | gen.const("close", int_t, 3, "close")], \ 13 | "An enum to set the operation used by the filter") 14 | # The values have to be identical to the cv::MORPH_* consts. 15 | gen.add("operation", int_t, 0, "Operation executed by the filter", 0, 0, 3, \ 16 | edit_method=operations) 17 | 18 | gen.add("num_iterations", int_t, 0, \ 19 | "Number of iterations", 20 | 1, 0, 10) 21 | 22 | exit(gen.generate(PACKAGE, "morphology_filter", "Parameters")) 23 | -------------------------------------------------------------------------------- /src/morphology_filter/include/morphology_filter/groupbox.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the GroupBox class. 3 | */ 4 | 5 | #ifndef MORPHOLOGY_FILTER_GROUPBOX_H 6 | #define MORPHOLOGY_FILTER_GROUPBOX_H 7 | 8 | // std. 9 | #include 10 | 11 | // Qt. 12 | #include 13 | #include 14 | 15 | // Headers of this project. 16 | #include 17 | #include "observed_parameter_manager.h" 18 | 19 | // Forward declarations. 20 | class QWidget; 21 | class QGridLayout; 22 | class QLabel; 23 | class QComboBox; 24 | class QSpinBox; 25 | 26 | 27 | namespace morphology_filter { 28 | 29 | /** \brief QGroupBox for accessing the parameters of a MorphologyFilter object. 30 | */ 31 | class GroupBox : public QGroupBox, public object_detection::Observer { 32 | Q_OBJECT 33 | 34 | public: 35 | /** \brief Creates and initializes the GroupBox. 36 | * 37 | * \param[in] parent Pointer to QWidget that serves as a parent of the 38 | * GroupBox. 39 | */ 40 | explicit GroupBox(QWidget* parent = nullptr); 41 | 42 | /** \brief Updates the view of the GroupBox so that it displays the current 43 | * values of the parameters. 44 | */ 45 | void update() override; 46 | 47 | /** \brief Sets the parameter-manager of the MorphologyFilter. 48 | * 49 | * The ObservedParameterManager manages the parameters of a MorphologyFilter 50 | * object. 51 | * The manager represents the Model of the MVC-pattern, while the GroupBox 52 | * represents the View and the Controller. 53 | */ 54 | void setParameterManager(ObservedParameterManager* const); 55 | 56 | private slots: 57 | void requestOperationChange(const QString&) const; 58 | void requestNumIterationsChange(const int) const; 59 | 60 | private: 61 | // Initialization functions. 62 | void initOperationWidgets(); 63 | void initNumIterationsWidgets(); 64 | 65 | QGridLayout* createMainLayout() const; 66 | 67 | // Static consts. 68 | static const std::map operation_mapping; 69 | 70 | // Data members and widgets. 71 | ObservedParameterManager* param_manager_ {nullptr}; 72 | 73 | QLabel* op_label_; 74 | QComboBox* op_combo_; 75 | 76 | QLabel* iter_label_; 77 | QSpinBox* iter_spinbox_; 78 | }; 79 | 80 | } // morphology_filter 81 | 82 | #endif // MORPHOLOGY_FILTER_GROUPBOX_H 83 | -------------------------------------------------------------------------------- /src/morphology_filter/include/morphology_filter/morphology_filter.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the MorphologyFilter class. 3 | */ 4 | 5 | #ifndef MORPHOLOGY_FILTER_MORPHOLOGY_FILTER_H 6 | #define MORPHOLOGY_FILTER_MORPHOLOGY_FILTER_H 7 | 8 | // object-detection headers. 9 | #include 10 | 11 | 12 | namespace morphology_filter { 13 | 14 | class ParameterManager; 15 | 16 | /** \brief Filter for applying morphological transformations on images. */ 17 | class MorphologyFilter : public object_detection_2d::Filter { 18 | public: 19 | /** \brief Creates a MorphologyFilter and initializes its parameter-manager. 20 | * 21 | * \param[in] pm Pointer to a valid ParameterManager. 22 | */ 23 | explicit MorphologyFilter(ParameterManager* pm); 24 | 25 | /** \brief Applies a morphological transformation on an image. 26 | * 27 | * \param[in] image Image on which the morphological transformation is 28 | * applied. 29 | * \return New image containing the transformed input image. 30 | */ 31 | cv::Mat filter(const cv::Mat& image) override; 32 | 33 | private: 34 | ParameterManager* param_manager_; 35 | }; 36 | 37 | } // morphology_filter 38 | 39 | #endif // MORPHOLOGY_FILTER_MORPHOLOGY_FILTER_H 40 | -------------------------------------------------------------------------------- /src/morphology_filter/include/morphology_filter/observed_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ObservedParameterManager class. 3 | */ 4 | 5 | #ifndef MORPHOLOGY_FILTER_OBSERVED_PARAMETER_MANAGER_H 6 | #define MORPHOLOGY_FILTER_OBSERVED_PARAMETER_MANAGER_H 7 | 8 | // Headers of this project. 9 | #include 10 | #include 11 | #include "parameter_manager.h" 12 | 13 | 14 | namespace cv { 15 | class FileStorage; 16 | } 17 | 18 | namespace morphology_filter { 19 | 20 | /** \brief Observable parameter-manager for a MorphologyFilter object. 21 | * 22 | * This ParameterManager inherits from object_detection:Subject, 23 | * making it possible to register observers for it. 24 | * It also inherits from object_detection_2d::Parametrizable. 25 | * 26 | * Using this parameter-manager makes it possible to access the parameters 27 | * of a MorphologyFilter via the object_detection_2d::GUI. 28 | */ 29 | class ObservedParameterManager : public ParameterManager, 30 | public object_detection::Subject, 31 | public object_detection_2d::Parametrizable { 32 | public: 33 | // 34 | // Parametrizable interface. 35 | // 36 | 37 | /** \brief Writes all parameters to the given FileStorage object. */ 38 | void writeParametersToStorage(cv::FileStorage&) const override; 39 | 40 | /** \brief Reads all parameters from the given FileStorage object 41 | * and sets its parameters accordingly. 42 | */ 43 | void setParametersFromStorage(cv::FileStorage&) override; 44 | 45 | // 46 | // Setters. 47 | // 48 | 49 | /** \brief Sets the operation parameter. */ 50 | void setOperation(Operation); 51 | /** \brief Sets the number-iterations parameter. */ 52 | void setNumIterations(int); 53 | }; 54 | 55 | } // morphology_filter 56 | 57 | #endif // MORPHOLOGY_FILTER_OBSERVED_PARAMETER_MANAGER_H 58 | -------------------------------------------------------------------------------- /src/morphology_filter/include/morphology_filter/parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ParameterManager class. 3 | */ 4 | 5 | /* TODO: Maybe it's better to make the ParameterManager class non-abstract. 6 | * A way to do this would be to give the MorphologyFilter a ParameterManager as a 7 | * member (not just a reference) and to give the ParameterManager object 8 | * a member that is the only one that is allowed to set the parameters, 9 | * therefore encapsulating how the parameters are set (from a reconfigure- 10 | * callback or through some request of an observer). 11 | * The MorphologyFilter would than provide a method for setting this setter-object 12 | * in the filter's parameter-manager object. 13 | */ 14 | 15 | #ifndef MORPHOLOGY_FILTER_PARAMETER_MANAGER_H 16 | #define MORPHOLOGY_FILTER_PARAMETER_MANAGER_H 17 | 18 | #include 19 | 20 | 21 | namespace morphology_filter { 22 | 23 | /** \brief Parameter-manager for BinaryDetector objects. 24 | * 25 | * A ParameterManager object manages the parameters of 26 | * a BinaryDetector object and provides getter- and setter-methods for 27 | * accessing those parameters. 28 | */ 29 | class ParameterManager { 30 | public: 31 | /** \brief Enumeration for values that can be assigned to the operation 32 | * parameter. 33 | */ 34 | enum class Operation { 35 | erode = cv::MORPH_ERODE, 36 | dilate = cv::MORPH_DILATE, 37 | open = cv::MORPH_OPEN, 38 | close = cv::MORPH_CLOSE 39 | }; 40 | 41 | // 42 | // Limits / Consts. 43 | // 44 | /** \brief Lower limit of the number-iterations parameter. */ 45 | static constexpr int min_num_iterations {0}; 46 | /** \brief Upper limit of the number-iterations parameter. */ 47 | static constexpr int max_num_iterations {10}; 48 | 49 | // 50 | // Getters & Setters. 51 | // 52 | 53 | /** \brief Returns the operation parameter, i.e. the morphological 54 | * transformation (e.g. 'dilate') that is applied. 55 | */ 56 | Operation operation() const { return operation_; } 57 | /** \brief Sets the operation parameter. */ 58 | void setOperation(Operation); 59 | 60 | /** \brief Returns the number-iterations parameter, i.e. the 61 | * the number of iterations used in the transformation. 62 | */ 63 | int numIterations() const { return num_iterations_; } 64 | /** \brief Sets the number-iterations parameter. */ 65 | void setNumIterations(int); 66 | 67 | private: 68 | // Parameters. 69 | Operation operation_ {Operation::open}; 70 | int num_iterations_ {1}; 71 | }; 72 | 73 | } // morphology_filter 74 | 75 | #endif // MORPHOLOGY_FILTER_PARAMETER_MANAGER_H 76 | -------------------------------------------------------------------------------- /src/morphology_filter/include/morphology_filter/reconfigure_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ReconfigureParameterManager class. 3 | */ 4 | 5 | #ifndef MORPHOLOGY_FILTER_RECONFIGURE_PARAMETER_MANAGER_H 6 | #define MORPHOLOGY_FILTER_RECONFIGURE_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include // uint32_t 10 | 11 | // dynamic_reconfigure. 12 | #include 13 | #include 14 | 15 | // Headers of this package. 16 | #include 17 | 18 | 19 | namespace morphology_filter { 20 | 21 | /** \brief dynamic_reconfigure parameter-manager for MorphologyFilter objects. 22 | * 23 | * A ReconfigureParameterManager object allows to manage the parameters 24 | * of a MorphologyFilter object via a corresponding set of dynamic_reconfigure 25 | * parameters. 26 | */ 27 | class ReconfigureParameterManager : public ParameterManager { 28 | public: 29 | /** \brief Initializes the dynamic_reconfigure server and registers a callback 30 | * for this server. 31 | */ 32 | ReconfigureParameterManager(); 33 | 34 | private: 35 | void reconfigureCallback(morphology_filter::ParametersConfig& config, 36 | uint32_t); 37 | 38 | // Dynamic reconfigure server & callback. 39 | dynamic_reconfigure::Server::CallbackType 40 | reconfigure_callback_; 41 | dynamic_reconfigure::Server 42 | reconfigure_server_; 43 | }; 44 | 45 | } // morphology_filter 46 | 47 | #endif // MORPHOLOGY_FILTER_RECONFIGURE_PARAMETER_MANAGER_H 48 | -------------------------------------------------------------------------------- /src/morphology_filter/params/pink_pad.yaml: -------------------------------------------------------------------------------- 1 | !!python/object/new:dynamic_reconfigure.encoding.Config 2 | dictitems: 3 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 4 | dictitems: 5 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 6 | state: [] 7 | id: 0 8 | name: Default 9 | num_iterations: 1 10 | operation: 2 11 | parameters: !!python/object/new:dynamic_reconfigure.encoding.Config 12 | state: [] 13 | parent: 0 14 | state: true 15 | type: '' 16 | state: [] 17 | num_iterations: 1 18 | operation: 2 19 | state: [] 20 | -------------------------------------------------------------------------------- /src/morphology_filter/src/morphology_filter.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the MorphologyFilter class. 3 | */ 4 | 5 | // std 6 | #include // runtime_error 7 | 8 | // ROS & OpenCV. 9 | #include // cv::Mat 10 | #include // morphologyEx() 11 | 12 | // Our headers. 13 | #include 14 | #include 15 | 16 | using namespace std; 17 | 18 | 19 | namespace morphology_filter { 20 | 21 | MorphologyFilter::MorphologyFilter(ParameterManager* pm) 22 | : param_manager_ {pm} 23 | { 24 | if (!pm) 25 | throw runtime_error {"MorphologyFilter: nullptr given to constructor"}; 26 | } 27 | 28 | 29 | cv::Mat MorphologyFilter::filter(const cv::Mat& image) 30 | { 31 | cv::Mat filtered_image; 32 | cv::morphologyEx(image, filtered_image, 33 | static_cast(param_manager_->operation()), 34 | cv::Mat{}, // default (3x3) kernel 35 | cv::Point{-1, -1}, // default (centered) anchor 36 | param_manager_->numIterations()); 37 | return filtered_image; 38 | } 39 | 40 | } // morphology_filter 41 | -------------------------------------------------------------------------------- /src/morphology_filter/src/observed_parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ObservedParameterManager class. 3 | */ 4 | 5 | // std. 6 | #include 7 | 8 | // OpenCV. 9 | #include // FileStorage 10 | 11 | // Headers of this package. 12 | #include 13 | 14 | using namespace std; 15 | 16 | 17 | namespace morphology_filter { 18 | 19 | namespace { 20 | 21 | string fullParamName(const string& param_name) 22 | { 23 | return "MorphologyFilter-" + param_name; 24 | } 25 | 26 | } // anonymous namespace 27 | 28 | 29 | //////////////////////////////////////////////////////////////////////////////// 30 | // 31 | // Parametrizable interface. 32 | // 33 | 34 | void ObservedParameterManager::writeParametersToStorage(cv::FileStorage& fs) 35 | const 36 | { 37 | auto& fpn = fullParamName; // convenience shortcut 38 | fs << fpn("operation") << static_cast(operation()) 39 | << fpn("num_iterations") << numIterations(); 40 | } 41 | 42 | 43 | void ObservedParameterManager::setParametersFromStorage(cv::FileStorage& fs) 44 | { 45 | auto& fpn = fullParamName; 46 | 47 | { 48 | auto fn = fs[fpn("operation")]; // file-node 49 | if (!fn.empty()) 50 | setOperation(static_cast((int)fn)); 51 | } 52 | 53 | { 54 | auto fn = fs[fpn("num_iterations")]; 55 | if (!fn.empty()) 56 | setNumIterations((int)fn); 57 | } 58 | } 59 | 60 | 61 | //////////////////////////////////////////////////////////////////////////////// 62 | // 63 | // Setters. 64 | // 65 | 66 | void ObservedParameterManager::setOperation(Operation op) 67 | { 68 | ParameterManager::setOperation(op); 69 | notify(); 70 | } 71 | 72 | 73 | void ObservedParameterManager::setNumIterations(int val) 74 | { 75 | ParameterManager::setNumIterations(val); 76 | notify(); 77 | } 78 | 79 | } // morphology_filter 80 | -------------------------------------------------------------------------------- /src/morphology_filter/src/parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ParameterManager class. 3 | */ 4 | 5 | // std. 6 | #include // runtime_error 7 | #include // min(), max() 8 | 9 | // Headers of this project. 10 | #include 11 | 12 | 13 | namespace { 14 | 15 | inline int truncateToRange(int val, int min, int max) 16 | { 17 | return std::min(std::max(val, min), max); 18 | } 19 | 20 | } // anonymous ns 21 | 22 | 23 | namespace morphology_filter { 24 | 25 | void ParameterManager::setOperation(const Operation op) 26 | { 27 | switch (op) { 28 | case Operation::erode: 29 | case Operation::dilate: 30 | case Operation::open: 31 | case Operation::close: 32 | break; 33 | default: 34 | throw std::runtime_error 35 | {"Invalid value passed to ParameterManager::setOperation"}; 36 | } 37 | 38 | operation_ = op; 39 | } 40 | 41 | 42 | void ParameterManager::setNumIterations(int val) 43 | { 44 | num_iterations_ = truncateToRange(val, min_num_iterations, max_num_iterations); 45 | } 46 | 47 | } // morphology_filter 48 | -------------------------------------------------------------------------------- /src/morphology_filter/src/reconfigure_parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ReconfigureParameterManager class. 3 | */ 4 | 5 | // std. 6 | #include // bind(), placeholders 7 | 8 | // object-detection. 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | 15 | namespace morphology_filter { 16 | 17 | ReconfigureParameterManager::ReconfigureParameterManager() 18 | : reconfigure_callback_{bind( 19 | &ReconfigureParameterManager::reconfigureCallback, this, 20 | placeholders::_1, placeholders::_2)} 21 | { 22 | reconfigure_server_.setCallback(reconfigure_callback_); 23 | } 24 | 25 | 26 | void ReconfigureParameterManager::reconfigureCallback( 27 | morphology_filter::ParametersConfig& config, uint32_t) 28 | { 29 | setOperation(static_cast(config.operation)); 30 | setNumIterations(config.num_iterations); 31 | } 32 | 33 | } // morphology_filter 34 | -------------------------------------------------------------------------------- /src/object_detection/include/object_detection/observer.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the Observer class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_OBSERVER_H 6 | #define OBJECT_DETECTION_OBSERVER_H 7 | 8 | // TODO: Unregister from subject in destructor. 9 | 10 | 11 | namespace object_detection { 12 | 13 | /** \brief Observer according to the OBSERVER pattern. */ 14 | class Observer { 15 | public: 16 | virtual ~Observer() {} 17 | 18 | /** \brief Updates the view on the Subject that the Observer presents. */ 19 | virtual void update() = 0; 20 | }; 21 | 22 | } // object_detection 23 | 24 | #endif // OBJECT_DETECTION_OBSERVER_H 25 | -------------------------------------------------------------------------------- /src/object_detection/include/object_detection/subject.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the Subject class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_SUBJECT_H 6 | #define OBJECT_DETECTION_SUBJECT_H 7 | 8 | // std. 9 | #include 10 | 11 | // Headers of this project. 12 | #include "observer.h" 13 | 14 | // TODO: Allow observers to unregister. 15 | 16 | 17 | namespace object_detection { 18 | 19 | /** \brief Subject according to the OBSERVER pattern. */ 20 | class Subject { 21 | public: 22 | /** \brief Adds an Observer to the internal vector of Observers. */ 23 | void addObserver(Observer* observer); 24 | 25 | /** \brief Calls the update method of each registered Observer. */ 26 | void notify() const; 27 | 28 | private: 29 | std::vector observers_; 30 | }; 31 | 32 | inline void Subject::addObserver(Observer* observer) 33 | { 34 | if (observer) 35 | observers_.push_back(observer); 36 | } 37 | 38 | inline void Subject::notify() const 39 | { 40 | for (auto observer : observers_) 41 | observer->update(); 42 | } 43 | 44 | } // object_detection 45 | 46 | #endif // OBJECT_DETECTION_SUBJECT_H 47 | -------------------------------------------------------------------------------- /src/object_detection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | object_detection 4 | 0.0.0 5 | The object_detection package 6 | 7 | 8 | 9 | 10 | janosch 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/object_detection_2d/include/object_detection_2d/definition_director.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the DefinitionDirector class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_2D_DEFINITION_DIRECTOR_H 6 | #define OBJECT_DETECTION_2D_DEFINITION_DIRECTOR_H 7 | 8 | #include 9 | #include 10 | 11 | 12 | namespace object_detection_2d { 13 | 14 | class GUI; 15 | class Filter; 16 | class Detector; 17 | 18 | /** \brief Director for coordinating the components that are involved in the 19 | * creation of parameter-files (definition files) for an object recognition 20 | * system. 21 | * 22 | * An object of this class keeps references to the components of the system. 23 | * It receives images over the ROS network in one of its member functions 24 | * and passes data between the components. 25 | */ 26 | class DefinitionDirector { 27 | public: 28 | /** \brief Sets up the ROS communication. */ 29 | DefinitionDirector(); 30 | 31 | // TODO: gui_ & detector_ should be public. (?!) 32 | /** \brief Sets the GUI component. */ 33 | void setGUI(GUI*); 34 | /** \brief Adds a Filter component. */ 35 | void addFilter(Filter*); 36 | /** \brief Sets the Detector component. */ 37 | void setDetector(Detector*); 38 | 39 | private: 40 | void imageCallback(const sensor_msgs::Image::ConstPtr& image_msg) const; 41 | 42 | // Data members. 43 | ros::NodeHandle node_handle_; 44 | ros::Subscriber image_sub_; 45 | 46 | GUI* gui_ {nullptr}; 47 | std::vector filters_; 48 | Detector* detector_ {nullptr}; 49 | }; 50 | 51 | } // object_detection_2d 52 | 53 | #endif // OBJECT_DETECTION_2D_DEFINITION_DIRECTOR_H 54 | -------------------------------------------------------------------------------- /src/object_detection_2d/include/object_detection_2d/detection_director.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the DetectionDirector class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_2D_DETECTION_DIRECTOR_H 6 | #define OBJECT_DETECTION_2D_DETECTION_DIRECTOR_H 7 | 8 | #include 9 | #include 10 | 11 | 12 | namespace object_detection_2d { 13 | 14 | class Filter; 15 | class Detector; 16 | 17 | 18 | /** \brief Director for coordinating the components of a 2D object recognition 19 | * system that are involved in the detection of objects based on a given 20 | * parameter-file. 21 | * 22 | * An object of this class keeps references to the components of the system. 23 | * It receives images over the ROS network in one of its member functions, 24 | * passes data between the components, and publishes the detected objects. 25 | */ 26 | class DetectionDirector { 27 | public: 28 | /** \brief Sets up the ROS communication. */ 29 | DetectionDirector(); 30 | 31 | /** \brief Adds a Filter component. */ 32 | void addFilter(Filter*); 33 | /** \brief Sets the Detector component. */ 34 | void setDetector(Detector*); 35 | 36 | private: 37 | void imageCallback(const sensor_msgs::Image::ConstPtr& image_msg) const; 38 | 39 | // Data members. 40 | ros::NodeHandle node_handle_; 41 | ros::Subscriber image_sub_; 42 | ros::Publisher obj_pub_; 43 | 44 | std::vector filters_; 45 | Detector* detector_ {nullptr}; 46 | }; 47 | 48 | } // object_detection_2d 49 | 50 | #endif // OBJECT_DETECTION_2D_DETECTION_DIRECTOR_H 51 | -------------------------------------------------------------------------------- /src/object_detection_2d/include/object_detection_2d/detector.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the Detector class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_2D_DETECTOR_H 6 | #define OBJECT_DETECTION_2D_DETECTOR_H 7 | 8 | #include // Mat, Rect 9 | #include 10 | 11 | 12 | namespace object_detection_2d { 13 | 14 | /** \brief Abstract detector class for detecting objects in 2D images. */ 15 | class Detector { 16 | public: 17 | /** \brief Detects objects in 2D image. 18 | * 19 | * \param[in] image Image in which the detector looks for objects. 20 | * \return Array containing the objects found. 21 | */ 22 | virtual object_detection_2d_msgs::DetectedObject2DArray detect( 23 | const cv::Mat& image) = 0; 24 | 25 | /** \brief Callback function for processing an area of the image that was 26 | * selected by the user. 27 | */ 28 | virtual void processAreaSelection(const cv::Rect& rect) {} 29 | 30 | // TODO: Do we need a virtual destructor? 31 | }; 32 | 33 | } // object_detection_2d 34 | 35 | #endif // OBJECT_DETECTION_2D_DETECTOR_H 36 | -------------------------------------------------------------------------------- /src/object_detection_2d/include/object_detection_2d/filter.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the Filter class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_2D_FILTER_H 6 | #define OBJECT_DETECTION_2D_FILTER_H 7 | 8 | #include // Mat 9 | 10 | 11 | namespace object_detection_2d { 12 | 13 | /** \brief Abstract filter for filtering an image. */ 14 | class Filter { 15 | public: 16 | /** \brief Filters the given image. 17 | * \param[in] image Image that is filtered. 18 | * \return New image containing the filtered image. 19 | */ 20 | virtual cv::Mat filter(const cv::Mat& image) = 0; 21 | }; 22 | 23 | } // object_detection_2d 24 | 25 | #endif // OBJECT_DETECTION_2D_FILTER_H 26 | -------------------------------------------------------------------------------- /src/object_detection_2d/include/object_detection_2d/interactive_image.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the InteractiveImage class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_2D_INTERACTIVE_IMAGE_H 6 | #define OBJECT_DETECTION_2D_INTERACTIVE_IMAGE_H 7 | 8 | // Qt. 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | // Forward declarations. 15 | class QRect; 16 | class QPaintEvent; 17 | class QMouseEvent; 18 | class QRubberBand; 19 | class QPoint; 20 | 21 | namespace cv { 22 | class Mat; 23 | } 24 | 25 | 26 | namespace object_detection_2d { 27 | 28 | /** \brief QWidget for rendering an image and allowing the user to 29 | * interact with it by clicking into it with his mouse. 30 | */ 31 | class InteractiveImage : public QWidget { 32 | Q_OBJECT 33 | 34 | public: 35 | explicit InteractiveImage(QWidget* parent = nullptr); 36 | 37 | /** \brief Renders given image. */ 38 | void showImage(const cv::Mat& cv_image); 39 | 40 | /** \brief Returns the ideal size of the widget. */ 41 | QSize sizeHint() const; // override?! 42 | 43 | protected: 44 | void paintEvent(QPaintEvent*) override; 45 | void mousePressEvent(QMouseEvent*) override; 46 | void mouseMoveEvent(QMouseEvent*) override; 47 | void mouseReleaseEvent(QMouseEvent*) override; 48 | 49 | signals: 50 | void mousePressSignal(const QMouseEvent*); 51 | void mouseMoveSignal(const QMouseEvent*); 52 | void mouseReleaseSignal(const QMouseEvent*); 53 | void areaSelected(const QRect& area); 54 | 55 | private: 56 | void copyToQtImage(const cv::Mat& cv_image); 57 | 58 | // Data members. 59 | QImage q_image_; 60 | QRubberBand* rubber_band_; 61 | bool image_initialized_; 62 | QPoint selection_origin_; 63 | }; 64 | 65 | } // object_detection_2d 66 | 67 | #endif // OBJECT_DETECTION_2D_INTERACTIVE_IMAGE_H 68 | -------------------------------------------------------------------------------- /src/object_detection_2d/include/object_detection_2d/parameter_storage.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ParameterStorage class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_2D_PARAMETER_STORAGE_H 6 | #define OBJECT_DETECTION_2D_PARAMETER_STORAGE_H 7 | 8 | // std. 9 | #include 10 | #include 11 | 12 | // object_detection*. 13 | #include "parametrizable.h" 14 | 15 | 16 | namespace object_detection_2d { 17 | 18 | /** \brief Class for handling the saving/loading of parameters 19 | * to/from parameter files. 20 | */ 21 | class ParameterStorage { 22 | public: 23 | /** \brief Registers a Parametrizable object to the internal vector of 24 | * references. 25 | */ 26 | void addParametrizable(Parametrizable*); 27 | 28 | /** \brief Saves the parameters of all registered Parametrizable objects 29 | * to a parameter file. 30 | * 31 | * The function creates the file with the given name and passes a handle 32 | * to the file to all registered Parametrizable objects, asking them to 33 | * write their parameters to the file. 34 | * 35 | * \param[in] filename Name of the parameter-file. 36 | * \return true if the file has been created successfully, false otherwise. 37 | */ 38 | bool saveParameters(const std::string& filename) const; 39 | 40 | /** \brief Opens a parameter-file and asks all registered Parametrizable 41 | * objects to write their parameters to that file. 42 | * 43 | * \param[in] filename Name of the parameter-file. 44 | * \return true if the file with the given name has been opened successfully, 45 | * false otherwise. 46 | */ 47 | bool loadParameters(const std::string& filename) const; 48 | 49 | private: 50 | std::vector parametrizables_; 51 | }; 52 | 53 | } // object_detection_2d 54 | 55 | #endif // OBJECT_DETECTION_2D_PARAMETER_STORAGE_H 56 | -------------------------------------------------------------------------------- /src/object_detection_2d/include/object_detection_2d/parametrizable.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the Parametrizable class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_2D_PARAMETRIZABLE_H 6 | #define OBJECT_DETECTION_2D_PARAMETRIZABLE_H 7 | 8 | #include 9 | 10 | 11 | namespace object_detection_2d { 12 | 13 | /** \brief Class for objects that have parameters and are able to write 14 | * them to and load them from a parameter-file. 15 | */ 16 | class Parametrizable { 17 | // TODO: Do we need a virtual destructor? 18 | public: 19 | /** \brief Writes the parameters of the object to the given FileStorage. */ 20 | virtual void writeParametersToStorage(cv::FileStorage&) const = 0; 21 | 22 | /** \brief Reads the values of the parameters of the object from the 23 | * given FileStorage and sets the internal parameters accordingly. 24 | */ 25 | virtual void setParametersFromStorage(cv::FileStorage&) = 0; 26 | }; 27 | 28 | } // object_detection_2d 29 | 30 | #endif // OBJECT_DETECTION_2D_PARAMETRIZABLE_H 31 | -------------------------------------------------------------------------------- /src/object_detection_2d/include/object_detection_2d/ros_detector_wrapper.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ROSDetectorWrapper class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_2D_ROS_DETECTOR_WRAPPER_H 6 | #define OBJECT_DETECTION_2D_ROS_DETECTOR_WRAPPER_H 7 | 8 | // ROS. 9 | #include 10 | #include 11 | 12 | // object_detection_*. 13 | #include 14 | 15 | 16 | namespace object_detection_2d { 17 | 18 | class Detector; 19 | 20 | /** \brief ROS wrapper for objects of the Detector class. 21 | * 22 | * A ROSDetectorWrapper object integrates Detector objects into the ROS 23 | * framework by providing the necessary subscribers, publishers, and event 24 | * handlers. 25 | */ 26 | class ROSDetectorWrapper { 27 | public: 28 | /** \brief Sets the internal Detector-reference and initializes 29 | * the ROS communication. 30 | * 31 | * \param[in] detector Valid pointer to the Detector object that is integrated 32 | * into the ROS framework. The ROSDetectorWrapper does not own this object. 33 | */ 34 | explicit ROSDetectorWrapper(Detector* detector); 35 | 36 | private: 37 | void initRosCommunication(); 38 | 39 | void imageCallback(const sensor_msgs::Image::ConstPtr& image_msg) const; 40 | void processImageMsg(const sensor_msgs::Image::ConstPtr& image_msg) const; 41 | 42 | void areaSelectionCallback( 43 | const object_detection_2d::Rect2D::ConstPtr& rect) const; 44 | 45 | // Data members. 46 | ros::NodeHandle nh_; 47 | ros::Subscriber image_sub_; 48 | ros::Subscriber selection_sub_; 49 | ros::Publisher objects_pub_; 50 | 51 | Detector* detector_; 52 | }; 53 | 54 | } // object_detection_2d 55 | 56 | #endif // OBJECT_DETECTION_2D_ROS_DETECTOR_WRAPPER_H 57 | -------------------------------------------------------------------------------- /src/object_detection_2d/include/object_detection_2d/ros_filter_wrapper.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ROSFilterWrapper class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_2D_ROS_FILTER_WRAPPER_H 6 | #define OBJECT_DETECTION_2D_ROS_FILTER_WRAPPER_H 7 | 8 | #include 9 | #include 10 | 11 | namespace object_detection_2d { 12 | 13 | class Filter; 14 | 15 | 16 | /** \brief ROS wrapper for objects of the Filter class. 17 | * 18 | * A ROSFilterWrapper object integrates Filter objects into the ROS 19 | * framework by providing the necessary subscribers, publishers and event 20 | * handlers. 21 | */ 22 | class ROSFilterWrapper { 23 | public: 24 | /** \brief Sets the internal Filter-reference and initializes 25 | * the ROS communication. 26 | * 27 | * \param[in] filter Valid pointer to the Filter object that is integrated 28 | * into the ROS framework. The ROSFilterWrapper does not own this object. 29 | */ 30 | explicit ROSFilterWrapper(Filter* filter); 31 | 32 | private: 33 | void initRosCommunication(); 34 | 35 | void imageCallback(const sensor_msgs::Image::ConstPtr& image_msg) const; 36 | void processImageMsg(const sensor_msgs::Image::ConstPtr& image_msg) const; 37 | 38 | // Data members. 39 | ros::NodeHandle nh_; 40 | ros::Subscriber sub_; 41 | ros::Publisher pub_; 42 | 43 | Filter* filter_; 44 | }; 45 | 46 | } // object_detection_2d 47 | 48 | #endif // OBJECT_DETECTION_2D_ROS_FILTER_WRAPPER_H 49 | -------------------------------------------------------------------------------- /src/object_detection_2d/msg/MouseEvent.msg: -------------------------------------------------------------------------------- 1 | # Types. 2 | uint8 PRESS = 0 3 | uint8 MOVE = 1 4 | uint8 RELEASE = 2 5 | 6 | # Buttons. 7 | uint8 NO_BUTTON = 0 8 | uint8 LEFT_BUTTON = 1 9 | uint8 MIDDLE_BUTTON = 2 10 | uint8 RIGHT_BUTTON = 3 11 | uint8 UNKNOWN_BUTTON = 4 12 | 13 | uint8 type 14 | uint8 button # button that caused the event (== NO_BUTTON for MOVE) 15 | 16 | # Mouse position when event was generated. 17 | object_detection_2d/Point2D position 18 | -------------------------------------------------------------------------------- /src/object_detection_2d/msg/Point2D.msg: -------------------------------------------------------------------------------- 1 | int32 x 2 | int32 y 3 | -------------------------------------------------------------------------------- /src/object_detection_2d/msg/Rect2D.msg: -------------------------------------------------------------------------------- 1 | object_detection_2d/Point2D upper_left 2 | 3 | int32 width 4 | int32 height 5 | -------------------------------------------------------------------------------- /src/object_detection_2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | object_detection_2d 4 | 0.0.0 5 | The object_detection_2d package 6 | 7 | 8 | 9 | 10 | janosch 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | cv_bridge 45 | roscpp 46 | sensor_msgs 47 | object_detection_2d_msgs--> 48 | message_generation--> 49 | 50 | cv_bridge 51 | roscpp 52 | sensor_msgs 53 | object_detection_2d_msgs--> 54 | message_runtime--> 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /src/object_detection_2d/src/detection_director.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the DetectionDirector class. 3 | */ 4 | 5 | // OpenCV. 6 | #include 7 | 8 | // ROS. 9 | #include 10 | #include 11 | 12 | // Headers of this project. 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using object_detection_2d_msgs::DetectedObject2DArray; 19 | using namespace std; 20 | 21 | 22 | namespace object_detection_2d { 23 | 24 | DetectionDirector::DetectionDirector() 25 | { 26 | image_sub_ = node_handle_.subscribe("input_image", 1, 27 | &DetectionDirector::imageCallback, this); 28 | obj_pub_ = 29 | node_handle_.advertise("detected_objects_2d", 10); 30 | } 31 | 32 | 33 | void DetectionDirector::imageCallback( 34 | const sensor_msgs::Image::ConstPtr& image_msg) const 35 | { 36 | if (!detector_) 37 | return; 38 | 39 | // Convert image. 40 | cv_bridge::CvImageConstPtr image_ptr; 41 | try { 42 | image_ptr = 43 | cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8); 44 | } 45 | catch (const cv_bridge::Exception& e) { 46 | ROS_ERROR_STREAM("cv_bridge::Exception when converting image-msg: " 47 | << e.what()); 48 | return; 49 | } 50 | 51 | // Filter image. 52 | cv::Mat filtered_img {image_ptr->image.clone()}; 53 | for (auto f : filters_) 54 | filtered_img = f->filter(filtered_img); 55 | 56 | // Detect objects. 57 | DetectedObject2DArray objects {detector_->detect(filtered_img)}; 58 | if (!objects.objects.empty()) 59 | obj_pub_.publish(objects); 60 | } 61 | 62 | 63 | void DetectionDirector::addFilter(Filter* f) 64 | { 65 | if (f) 66 | filters_.push_back(f); 67 | } 68 | 69 | 70 | void DetectionDirector::setDetector(Detector* det) 71 | { 72 | detector_ = det; 73 | } 74 | 75 | } // object_detection_2d 76 | -------------------------------------------------------------------------------- /src/object_detection_2d/src/interactive_image.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the InteractiveImage class. 3 | */ 4 | 5 | // Qt. 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | // OpenCV. 16 | #include 17 | 18 | // Our headers. 19 | #include 20 | 21 | 22 | namespace object_detection_2d { 23 | 24 | InteractiveImage::InteractiveImage(QWidget* parent) 25 | : QWidget{parent}, 26 | rubber_band_{new QRubberBand{QRubberBand::Rectangle, this}}, 27 | image_initialized_{false} 28 | { 29 | setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); 30 | rubber_band_->hide(); 31 | } 32 | 33 | 34 | void InteractiveImage::paintEvent(QPaintEvent*) 35 | { 36 | QPainter painter{this}; 37 | painter.drawImage(QPoint{0, 0}, q_image_); 38 | } 39 | 40 | 41 | void InteractiveImage::mousePressEvent(QMouseEvent* event) 42 | { 43 | selection_origin_ = event->pos(); 44 | rubber_band_->setGeometry(QRect{selection_origin_, QSize{}}); 45 | rubber_band_->show(); 46 | setCursor(Qt::CrossCursor); 47 | 48 | emit mousePressSignal(event); 49 | } 50 | 51 | 52 | void InteractiveImage::mouseMoveEvent(QMouseEvent* event) 53 | { 54 | rubber_band_->setGeometry( 55 | QRect{selection_origin_, event->pos()}.normalized() 56 | ); 57 | 58 | emit mouseMoveSignal(event); 59 | } 60 | 61 | 62 | void InteractiveImage::mouseReleaseEvent(QMouseEvent* event) 63 | { 64 | rubber_band_->hide(); 65 | unsetCursor(); 66 | emit areaSelected(QRect{selection_origin_, event->pos()}); 67 | 68 | emit mouseReleaseSignal(event); 69 | } 70 | 71 | 72 | void InteractiveImage::showImage(const cv::Mat& cv_image) 73 | { 74 | copyToQtImage(cv_image); 75 | update(); 76 | } 77 | 78 | 79 | QSize InteractiveImage::sizeHint() const 80 | { 81 | return q_image_.size(); 82 | } 83 | 84 | 85 | void InteractiveImage::copyToQtImage(const cv::Mat& cv_image) 86 | { 87 | if (!image_initialized_) { 88 | q_image_ = QImage{QSize{cv_image.cols, cv_image.rows}, QImage::Format_RGB888}; 89 | image_initialized_ = true; 90 | updateGeometry(); 91 | } 92 | 93 | cv::Mat cv_header_to_qt_image( 94 | cv::Size{q_image_.width(), q_image_.height()}, 95 | CV_8UC3, 96 | q_image_.bits() 97 | ); 98 | cv::cvtColor(cv_image, cv_header_to_qt_image, cv::COLOR_BGR2RGB); 99 | } 100 | 101 | } // object_detection_2d 102 | -------------------------------------------------------------------------------- /src/object_detection_2d/src/parameter_storage.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ParameterStorage class. 3 | */ 4 | 5 | // std. 6 | #include 7 | 8 | // OpenCV. 9 | #include 10 | 11 | // object_detection_*. 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | 17 | 18 | namespace object_detection_2d { 19 | 20 | void ParameterStorage::addParametrizable(Parametrizable* obj_ptr) 21 | { 22 | parametrizables_.push_back(obj_ptr); 23 | } 24 | 25 | 26 | bool ParameterStorage::saveParameters(const string& filename) const 27 | try { 28 | cv::FileStorage storage {filename, cv::FileStorage::WRITE}; 29 | if (storage.isOpened()) { 30 | for (auto& obj : parametrizables_) 31 | obj->writeParametersToStorage(storage); 32 | return true; 33 | } 34 | return false; 35 | } 36 | catch (const cv::Exception& e) { // e.g. when a parametrizable uses invalid keys 37 | return false; 38 | } 39 | 40 | 41 | bool ParameterStorage::loadParameters(const string& filename) const 42 | try { 43 | cv::FileStorage storage {filename, cv::FileStorage::READ}; 44 | if (storage.isOpened()) { 45 | for (Parametrizable* obj : parametrizables_) 46 | obj->setParametersFromStorage(storage); 47 | return true; 48 | } 49 | return false; 50 | } 51 | catch (const cv::Exception& e) { 52 | return false; 53 | } 54 | 55 | } // object_detection_2d 56 | -------------------------------------------------------------------------------- /src/object_detection_2d/src/ros_detector_wrapper.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ROSDetectorWrapper class. 3 | */ 4 | 5 | // std. 6 | #include // runtime_error 7 | 8 | // ROS & OpenCV. 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | // Object-detection. 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using object_detection_2d_msgs::DetectedObject2DArray; 21 | using object_detection_2d::Rect2D; 22 | 23 | 24 | namespace object_detection_2d { 25 | 26 | ROSDetectorWrapper::ROSDetectorWrapper(Detector* detector) 27 | : detector_ {detector} 28 | { 29 | if (!detector) 30 | throw std::runtime_error {"ROSDetectorWrapper: nullptr passed to constructor"}; 31 | 32 | initRosCommunication(); 33 | } 34 | 35 | 36 | void ROSDetectorWrapper::initRosCommunication() 37 | { 38 | image_sub_ = 39 | nh_.subscribe("input_image", 1, &ROSDetectorWrapper::imageCallback, this); 40 | selection_sub_ = 41 | nh_.subscribe("selection_area", 10, 42 | &ROSDetectorWrapper::areaSelectionCallback, this); 43 | objects_pub_ = 44 | nh_.advertise("detected_objects_2d", 10); 45 | } 46 | 47 | 48 | void ROSDetectorWrapper::imageCallback( 49 | const sensor_msgs::Image::ConstPtr& image_msg) const 50 | { 51 | try { 52 | processImageMsg(image_msg); 53 | } 54 | catch (const cv_bridge::Exception& e) { 55 | ROS_ERROR_STREAM("cv_bridge::Exception when converting image: " 56 | << e.what()); 57 | return; 58 | } 59 | } 60 | 61 | 62 | void ROSDetectorWrapper::processImageMsg( 63 | const sensor_msgs::Image::ConstPtr& image_msg) const 64 | { 65 | cv_bridge::CvImageConstPtr cv_image = 66 | cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8); 67 | 68 | DetectedObject2DArray objects = detector_->detect(cv_image->image); 69 | if (!objects.objects.empty()) 70 | objects_pub_.publish(objects); 71 | } 72 | 73 | 74 | void ROSDetectorWrapper::areaSelectionCallback(const Rect2D::ConstPtr& rect) 75 | const 76 | { 77 | cv::Rect cv_rect {rect->upper_left.x, rect->upper_left.y, 78 | rect->width, rect->height}; 79 | detector_->processAreaSelection(cv_rect); 80 | } 81 | 82 | } // object_detection_2d 83 | -------------------------------------------------------------------------------- /src/object_detection_2d/src/ros_filter_wrapper.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ROSFilterWrapper class. 3 | */ 4 | 5 | // std. 6 | #include // runtime_error 7 | 8 | // ROS & OpenCV. 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | // Object-detection_*. 15 | #include 16 | #include 17 | 18 | 19 | namespace object_detection_2d { 20 | 21 | ROSFilterWrapper::ROSFilterWrapper(Filter* filter) 22 | : filter_ {filter} 23 | { 24 | if (!filter) 25 | throw std::runtime_error {"ROSFilterWrapper: nullptr passed to constructor"}; 26 | 27 | initRosCommunication(); 28 | } 29 | 30 | 31 | void ROSFilterWrapper::initRosCommunication() 32 | { 33 | sub_ = nh_.subscribe("input_image", 1, 34 | &ROSFilterWrapper::imageCallback, this); 35 | pub_ = nh_.advertise("output_image", 10); 36 | } 37 | 38 | 39 | void ROSFilterWrapper::imageCallback( 40 | const sensor_msgs::Image::ConstPtr& image_msg) const 41 | { 42 | try { 43 | processImageMsg(image_msg); 44 | } 45 | catch (const cv_bridge::Exception& e) { 46 | ROS_ERROR_STREAM("cv_bridge::Exception when converting image: " 47 | << e.what()); 48 | return; 49 | } 50 | } 51 | 52 | 53 | /* 54 | * Convert message to cv-image, filter the image, convert it back to a message, 55 | * and publish it. 56 | */ 57 | void ROSFilterWrapper::processImageMsg(const sensor_msgs::Image::ConstPtr& image_msg) const 58 | { 59 | cv_bridge::CvImagePtr cv_image = 60 | cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8); 61 | cv_image->image = filter_->filter(cv_image->image); 62 | pub_.publish(cv_image->toImageMsg()); 63 | } 64 | 65 | } // object_detection_2d 66 | -------------------------------------------------------------------------------- /src/object_detection_2d_msgs/msg/DetectedObject2D.msg: -------------------------------------------------------------------------------- 1 | string name 2 | 3 | # We use a polygon to represent the location of the object in pixel space. 4 | # This gives us a very general description of the object and many possibilities 5 | # for describing the contour of the object. 6 | # Polygon is also predefined in the geometry_msgs package. We set the 7 | # z-coordinate of the points in the polygon to zero since we work in pixel 8 | # space. 9 | geometry_msgs/Polygon polygon 10 | -------------------------------------------------------------------------------- /src/object_detection_2d_msgs/msg/DetectedObject2DArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | DetectedObject2D[] objects 3 | -------------------------------------------------------------------------------- /src/object_detection_2d_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | object_detection_2d_msgs 4 | 0.0.0 5 | The object_detection_2d_msgs package 6 | 7 | 8 | 9 | 10 | janosch 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | geometry_msgs 45 | std_msgs 46 | message_generation 47 | 48 | geometry_msgs 49 | std_msgs 50 | message_runtime 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/launch/feature_gui_definition.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/launch/feature_gui_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 14 | 15 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/launch/feature_node_definition.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 10 | 11 | 14 | 15 | 16 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/launch/feature_node_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/launch/hsv_definition.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/launch/hsv_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/launch/hsv_morph_binary_nodes_definition.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 24 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/launch/hsv_morph_binary_nodes_pinkpad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 14 | 17 | 18 | 19 | 20 | 25 | 28 | 29 | 30 | 31 | 36 | 37 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/launch/hsv_pinkpad_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/params/hsv_pinkpad.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 136 4 | 179 5 | 139 6 | 255 7 | 27 8 | 255 9 | 2 10 | 1 11 | 2.8054000000000001e-01 12 | 2.4878549999999997e+00 13 | 14 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/src/binary_detector_node.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief main function for the binary_detector node. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include 8 | 9 | // ROS. 10 | #include 11 | 12 | // Headers of this project. 13 | #include 14 | #include 15 | #include 16 | 17 | using object_detection_2d::ROSDetectorWrapper; 18 | using binary_detector::BinaryDetector; 19 | using binary_detector::ReconfigureParameterManager; 20 | using namespace std; 21 | 22 | 23 | void setupAndRun(int argc, char** argv) 24 | { 25 | ros::init(argc, argv, "binary_detector"); 26 | 27 | ReconfigureParameterManager pm; 28 | BinaryDetector detector {&pm}; 29 | ROSDetectorWrapper wrapper {&detector}; 30 | 31 | ros::spin(); 32 | } 33 | 34 | 35 | int main(int argc, char** argv) 36 | try 37 | { 38 | setupAndRun(argc, argv); 39 | return 0; 40 | } 41 | catch (const exception& e) 42 | { 43 | cerr << "std::exception: " << e.what() << '\n'; 44 | return 1; 45 | } 46 | catch (...) 47 | { 48 | cerr << "Unknown exception. Terminating program.\n"; 49 | return 1; 50 | } 51 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/src/feature_definition.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Node for creating parameter-files for the feature-detection system. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include 8 | #include // bind() 9 | 10 | // ROS. 11 | #include 12 | 13 | // Qt. 14 | #include 15 | 16 | // Headers of this project. 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | using object_detection_2d::GUI; 25 | using object_detection_2d::ParameterStorage; 26 | using object_detection_2d::DefinitionDirector; 27 | using feature_detector::FeatureDetector; 28 | using feature_detector::GroupBox; 29 | using feature_detector::ObservedParameterManager; 30 | using AreaSelectionCallback = GUI::AreaSelectionCallback; 31 | using namespace std; 32 | 33 | 34 | int main(int argc, char** argv) 35 | try { 36 | // Initialization. 37 | ros::init(argc, argv, "feature_definition"); 38 | QApplication app {argc, argv}; 39 | 40 | // Create detector with groupbox. 41 | ObservedParameterManager pm; 42 | FeatureDetector detector {&pm}; 43 | GroupBox* groupbox {new GroupBox}; 44 | groupbox->setParameterManager(&pm); 45 | pm.addObserver(groupbox); 46 | 47 | // Create parameter-storage. 48 | ParameterStorage ps; 49 | ps.addParametrizable(&pm); 50 | 51 | // Create gui. 52 | GUI gui; 53 | gui.setWindowTitle("Feature Definition"); 54 | gui.addRightWidget(groupbox); 55 | gui.setParameterStorage(&ps); 56 | gui.registerAreaSelectionCallback( 57 | AreaSelectionCallback{ 58 | bind(&FeatureDetector::processAreaSelection, &detector, placeholders::_1) 59 | }); 60 | gui.show(); 61 | 62 | // Create director. 63 | DefinitionDirector director; 64 | director.setDetector(&detector); 65 | director.setGUI(&gui); 66 | 67 | // Event loop. 68 | ros::Rate rate {10}; 69 | while (ros::ok()) { 70 | ros::spinOnce(); 71 | app.processEvents(); 72 | 73 | if (!gui.isVisible()) 74 | ros::shutdown(); 75 | 76 | rate.sleep(); 77 | } 78 | 79 | // Clean up. 80 | if (!ros::ok()) 81 | app.quit(); 82 | 83 | return 0; 84 | } 85 | catch (const exception& e) { 86 | cerr << "std::exception: " << e.what() << '\n'; 87 | return 1; 88 | } 89 | catch (...) { 90 | cerr << "Unknown exception caught.\n"; 91 | return 1; 92 | } 93 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/src/feature_detection.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Node for detecting objects with the feature-detection system, 3 | * based on a given parameter-file. 4 | */ 5 | 6 | // std. 7 | #include 8 | #include 9 | #include 10 | 11 | // ROS. 12 | #include 13 | 14 | // Headers of our project. 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using object_detection_2d::ParameterStorage; 21 | using object_detection_2d::DetectionDirector; 22 | using feature_detector::FeatureDetector; 23 | using feature_detector::ObservedParameterManager; 24 | 25 | using namespace std; 26 | 27 | 28 | void printUsage(const string& program_name) 29 | { 30 | cerr << "Usage: " << program_name << " \n"; 31 | } 32 | 33 | 34 | int main(int argc, char** argv) 35 | try { 36 | // Init ROS. 37 | ros::init(argc, argv, "feature_detection"); 38 | 39 | if (argc != 2) { 40 | printUsage(argv[0]); 41 | return 0; 42 | } 43 | 44 | // Create components of the system. 45 | ObservedParameterManager pm; // TODO: Should we use a special derived class for this? 46 | pm.setObjectName("dummy"); // TODO: Derive name from filename. 47 | FeatureDetector detector {&pm}; 48 | 49 | ParameterStorage storage; 50 | storage.addParametrizable(&pm); 51 | if (!storage.loadParameters(argv[1])) { 52 | cerr << "Cannot load parameters from file '" << argv[1] << "'\n"; 53 | return 1; 54 | } 55 | 56 | DetectionDirector director; 57 | director.setDetector(&detector); 58 | 59 | // Event loop. 60 | ros::spin(); 61 | return 0; 62 | } 63 | catch (const exception& e) { 64 | cerr << "std::exception: " << e.what() << '\n'; 65 | return 1; 66 | } 67 | catch (...) { 68 | cerr << "Unknown exception caught.\n"; 69 | return 1; 70 | } 71 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/src/feature_detector_node.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief main function for the feature_detector node. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include 8 | 9 | // ROS. 10 | #include 11 | 12 | // Object-detection. 13 | #include 14 | #include 15 | #include 16 | 17 | using object_detection_2d::ROSDetectorWrapper; 18 | using feature_detector::FeatureDetector; 19 | using feature_detector::ReconfigureParameterManager; 20 | using namespace std; 21 | 22 | 23 | void setupAndRun(int argc, char** argv) 24 | { 25 | ros::init(argc, argv, "feature_detector"); 26 | 27 | ReconfigureParameterManager pm; 28 | FeatureDetector detector {&pm}; 29 | ROSDetectorWrapper wrapper {&detector}; 30 | 31 | ros::spin(); 32 | } 33 | 34 | 35 | int main(int argc, char** argv) 36 | try { 37 | setupAndRun(argc, argv); 38 | return 0; 39 | } 40 | catch (const exception& e) { 41 | cerr << "std::exception: " << e.what() << '\n'; 42 | return 1; 43 | } 44 | catch (...) { 45 | cerr << "Unknown exception. Terminating program.\n"; 46 | return 1; 47 | } 48 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/src/hsv_detection.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Node for detecting objects with the hsv-detection system, 3 | * based on a given parameter-file. 4 | */ 5 | 6 | // std. 7 | #include 8 | #include 9 | #include 10 | 11 | // ROS. 12 | #include 13 | 14 | // Headers of this project. 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | using object_detection_2d::ParameterStorage; 25 | using object_detection_2d::DetectionDirector; 26 | using hsv_filter::HSVFilter; 27 | using morphology_filter::MorphologyFilter; 28 | using binary_detector::BinaryDetector; 29 | 30 | using namespace std; 31 | 32 | 33 | void printUsage(const string& program_name) 34 | { 35 | cerr << "Usage: " << program_name << " \n"; 36 | } 37 | 38 | 39 | int main(int argc, char** argv) 40 | try { 41 | // Init ROS. 42 | ros::init(argc, argv, "hsv_detection"); 43 | 44 | if (argc != 2) { 45 | printUsage(argv[0]); 46 | return 0; 47 | } 48 | 49 | // Create components of the system. 50 | hsv_filter::ObservedParameterManager hsv_pm; 51 | HSVFilter hsv_filter {&hsv_pm}; 52 | 53 | morphology_filter::ObservedParameterManager morph_pm; 54 | MorphologyFilter morph_filter {&morph_pm}; 55 | 56 | binary_detector::ObservedParameterManager binary_pm; 57 | binary_pm.setObjectName("dummy"); // TODO 58 | BinaryDetector detector {&binary_pm}; 59 | 60 | ParameterStorage storage; 61 | storage.addParametrizable(&hsv_pm); 62 | storage.addParametrizable(&morph_pm); 63 | storage.addParametrizable(&binary_pm); 64 | if (!storage.loadParameters(argv[1])) { 65 | cerr << "Cannot load parameters from file '" << argv[1] << "'\n"; 66 | return 1; 67 | } 68 | 69 | DetectionDirector director; 70 | director.addFilter(&hsv_filter); 71 | director.addFilter(&morph_filter); 72 | director.setDetector(&detector); 73 | 74 | // Event loop. 75 | ros::spin(); 76 | return 0; 77 | } 78 | catch (const exception& e) { 79 | cerr << "std::exception: " << e.what() << '\n'; 80 | return 1; 81 | } 82 | catch (...) { 83 | cerr << "Unknown exception caught.\n"; 84 | return 1; 85 | } 86 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/src/hsv_filter_node.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief main function for the hsv_filter node. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include 8 | 9 | // ROS. 10 | #include 11 | 12 | // Headers of this project. 13 | #include 14 | #include 15 | #include 16 | 17 | using object_detection_2d::ROSFilterWrapper; 18 | using hsv_filter::HSVFilter; 19 | using hsv_filter::ReconfigureParameterManager; 20 | using namespace std; 21 | 22 | 23 | void setupAndRun(int argc, char** argv) 24 | { 25 | ros::init(argc, argv, "hsv_filter"); 26 | 27 | ReconfigureParameterManager pm; 28 | HSVFilter filter {&pm}; 29 | ROSFilterWrapper wrapper {&filter}; 30 | 31 | ros::spin(); 32 | } 33 | 34 | 35 | int main(int argc, char** argv) 36 | try { 37 | setupAndRun(argc, argv); 38 | return 0; 39 | } 40 | catch (const exception& e) { 41 | cerr << "std::exception: " << e.what() << '\n'; 42 | return 1; 43 | } 44 | catch (...) { 45 | cerr << "Unknown exception. Terminating program.\n"; 46 | return 1; 47 | } 48 | -------------------------------------------------------------------------------- /src/object_detection_2d_nodes/src/morphology_filter_node.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief main function for the morphology_filter node. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include 8 | 9 | // ROS. 10 | #include 11 | 12 | // object-detection. 13 | #include 14 | #include 15 | #include 16 | 17 | using object_detection_2d::ROSFilterWrapper; 18 | using morphology_filter::MorphologyFilter; 19 | using morphology_filter::ReconfigureParameterManager; 20 | using namespace std; 21 | 22 | 23 | void setupAndRun(int argc, char** argv) 24 | { 25 | ros::init(argc, argv, "morphology_filter"); 26 | 27 | ReconfigureParameterManager pm; 28 | MorphologyFilter filter {&pm}; 29 | ROSFilterWrapper wrapper {&filter}; 30 | 31 | ros::spin(); 32 | } 33 | 34 | 35 | int main(int argc, char** argv) 36 | try { 37 | setupAndRun(argc, argv); 38 | return 0; 39 | } 40 | catch (const exception& e) { 41 | cerr << "std::exception: " << e.what() << '\n'; 42 | return 1; 43 | } 44 | catch (...) { 45 | cerr << "Unknown exception. Terminating program.\n"; 46 | return 1; 47 | } 48 | -------------------------------------------------------------------------------- /src/object_detection_2d_vis/include/object_detection_2d_vis/visualizer.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the Visualizer class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_2D_VIS_VISUALIZER_H 6 | #define OBJECT_DETECTION_2D_VIS_VISUALIZER_H 7 | 8 | // Qt. 9 | #include 10 | #include 11 | 12 | // ROS. 13 | #include 14 | #include 15 | 16 | class QMouseEvent; 17 | namespace object_detection_2d { class InteractiveImage; } 18 | 19 | 20 | namespace object_detection_2d_vis { 21 | 22 | /** \brief Interactive visualizer for showing images and capturing 23 | * mouse events. 24 | */ 25 | class Visualizer : public QObject { 26 | Q_OBJECT 27 | 28 | public: 29 | /** \brief Default constructor. 30 | * 31 | * Initializes ROS communication and the gui widgets. 32 | */ 33 | Visualizer(); 34 | 35 | /** \brief Returns true if the gui is visible, false if it has been closed. */ 36 | bool isVisible() const { return window_.isVisible(); } 37 | 38 | private slots: 39 | void mousePressSlot(const QMouseEvent*) const; 40 | void mouseMoveSlot(const QMouseEvent*) const; 41 | void mouseReleaseSlot(const QMouseEvent*) const; 42 | 43 | private: 44 | void initROSCommunication(); 45 | void initGUI(); 46 | void initVisualElements(); 47 | void connectSignalsAndSlots(); 48 | 49 | void imageCallback(const sensor_msgs::Image::ConstPtr& image_msg) const; 50 | void convertAndShowImageMsg(const sensor_msgs::Image::ConstPtr& image_msg) 51 | const; 52 | 53 | // Data members. 54 | ros::NodeHandle node_handle_; 55 | ros::Subscriber image_sub_; 56 | ros::Publisher mouse_pub_; 57 | 58 | QWidget window_; 59 | object_detection_2d::InteractiveImage* inter_image_; 60 | }; 61 | 62 | } // object_detection_2d_vis 63 | 64 | #endif // OBJECT_DETECTION_2D_VIS_VISUALIZER_H 65 | -------------------------------------------------------------------------------- /src/object_detection_2d_vis/launch/vis_area_selection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/object_detection_2d_vis/launch/visualizer.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/object_detection_2d_vis/src/visualizer_main.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief main function for running the Visualizer. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include 8 | 9 | // Qt. 10 | #include 11 | 12 | // ROS. 13 | #include 14 | 15 | // object_detection_*. 16 | #include 17 | 18 | using object_detection_2d_vis::Visualizer; 19 | using namespace std; 20 | 21 | 22 | /** \brief Representation of the visualizer-application. */ 23 | class Application { 24 | public: 25 | /** \brief Default constructor. 26 | * 27 | * Initializes the QApplication and Visualizer members. 28 | */ 29 | Application(int argc, char** argv) 30 | : q_app_ {argc, argv} {} 31 | 32 | /** \brief Processes events until the node is shut down. */ 33 | void run() 34 | { 35 | ros::Rate r {10}; 36 | while (ros::ok()) { 37 | processEvents(); 38 | r.sleep(); 39 | } 40 | } 41 | 42 | /** \brief Destructor. 43 | * 44 | * Closes the gui if necessary. 45 | */ 46 | ~Application() 47 | { 48 | q_app_.quit(); 49 | } 50 | 51 | private: 52 | /** \brief Processes ROS and Qt events. */ 53 | void processEvents() 54 | { 55 | ros::spinOnce(); 56 | q_app_.processEvents(); 57 | 58 | // Shutdown ros when gui is closed. 59 | if (!vis_.isVisible()) 60 | ros::shutdown(); 61 | } 62 | 63 | // Data members. 64 | QApplication q_app_; 65 | Visualizer vis_; 66 | }; 67 | 68 | 69 | /** \brief main function. 70 | * 71 | * Creates an Application object and lets it run. 72 | */ 73 | int main(int argc, char** argv) 74 | try { 75 | ros::init(argc, argv, "visualizer"); 76 | 77 | Application app {argc, argv}; 78 | app.run(); 79 | 80 | return 0; 81 | } 82 | catch (const std::exception& e) { 83 | cerr << "std::exception: " << e.what() << '\n'; 84 | return 1; 85 | } 86 | catch (...) { 87 | cerr << "Unknown exception caught. Terminating program.\n"; 88 | return 1; 89 | } 90 | -------------------------------------------------------------------------------- /src/object_detection_3d/include/object_detection_3d/definition_director.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the DefinitionDirector class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_3D_DEFINITION_DIRECTOR_H 6 | #define OBJECT_DETECTION_3D_DEFINITION_DIRECTOR_H 7 | 8 | // std. 9 | #include 10 | 11 | // ROS. 12 | #include 13 | 14 | // Our headers. 15 | #include "pcl_types.h" 16 | 17 | 18 | namespace object_detection_3d { 19 | 20 | class Filter; 21 | class Detector; 22 | class GUI; 23 | 24 | 25 | /** \brief Director for coordinating the components that are involved in the 26 | * creation of parameter-files (definition files) for a 3D object recognition 27 | * system. 28 | * 29 | * An object of this class keeps references to the components of the system. 30 | * It receives images over the ROS network in one of its member functions 31 | * and passes data between the components. 32 | */ 33 | class DefinitionDirector { 34 | public: 35 | /** \brief Sets up the ROS communication. */ 36 | DefinitionDirector(); 37 | 38 | /** \brief Sets the GUI component. */ 39 | void setGUI(GUI*); 40 | /** \brief Adds a Filter component. */ 41 | void addFilter(Filter*); 42 | /** \brief Sets the Detector component. */ 43 | void setDetector(Detector*); 44 | 45 | private: 46 | void cloudCallback(const PointCloudT::ConstPtr& cloud) const; 47 | 48 | // Data members. 49 | ros::NodeHandle node_handle_; 50 | ros::Subscriber subscriber_; 51 | GUI* gui_; 52 | std::vector filters_; 53 | Detector* detector_; 54 | }; 55 | 56 | } // object_detection_3d 57 | 58 | #endif // OBJECT_DETECTION_3D_DEFINITION_DIRECTOR_H 59 | -------------------------------------------------------------------------------- /src/object_detection_3d/include/object_detection_3d/detection_director.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the DetectionDirector class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_3D_DETECTION_DIRECTOR_H 6 | #define OBJECT_DETECTION_3D_DETECTION_DIRECTOR_H 7 | 8 | // std. 9 | #include 10 | 11 | // ROS. 12 | #include 13 | 14 | // Our headers. 15 | #include "pcl_types.h" 16 | 17 | 18 | namespace object_detection_3d { 19 | 20 | class Filter; 21 | class Detector; 22 | 23 | 24 | /** \brief Director for coordinating the components of a 3D object recognition 25 | * system that are involved in the detection of objects based on a given 26 | * parameter-file. 27 | * 28 | * An object of this class keeps references to the components of the system. 29 | * It receives images over the ROS network in one of its member functions, 30 | * passes data between the components, and publishes the detected objects. 31 | */ 32 | class DetectionDirector { 33 | public: 34 | /** \brief Sets up the ROS communication. */ 35 | DetectionDirector(); 36 | 37 | /** \brief Adds a Filter component. */ 38 | void addFilter(Filter* filter); 39 | /** \brief Sets the Detector component. */ 40 | void setDetector(Detector* detector); 41 | 42 | private: 43 | void cloudCallback(const PointCloudT::ConstPtr& cloud) const; 44 | 45 | // Data members. 46 | ros::NodeHandle node_handle_; 47 | ros::Subscriber subscriber_; 48 | ros::Publisher publisher_; 49 | std::vector filters_; 50 | Detector* detector_; 51 | }; 52 | 53 | } // object_detection_3d 54 | 55 | #endif // OBJECT_DETECTION_3D_DETECTION_DIRECTOR_H 56 | -------------------------------------------------------------------------------- /src/object_detection_3d/include/object_detection_3d/detector.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the Detector class. 3 | */ 4 | 5 | // TODO: Do we need a virtual destructor? 6 | 7 | #ifndef OBJECT_DETECTION_3D_DETECTOR_H 8 | #define OBJECT_DETECTION_3D_DETECTOR_H 9 | 10 | // Our headers. 11 | #include 12 | #include "pcl_types.h" 13 | 14 | 15 | namespace object_detection_3d { 16 | 17 | /** \brief Abstract detector class for detecting 3D objects in point clouds. */ 18 | class Detector { 19 | public: 20 | /** \brief Detects objects in a point cloud. 21 | * 22 | * \param[in] cloud Point cloud in which the detector looks for objects. 23 | * \return Array containing the objects found. 24 | */ 25 | virtual object_detection_3d_msgs::DetectedObject3DArray detect( 26 | const PointCloudT::ConstPtr& cloud) = 0; 27 | }; 28 | 29 | } // object_detection_3d 30 | 31 | #endif // OBJECT_DETECTION_3D_DETECTOR_H 32 | -------------------------------------------------------------------------------- /src/object_detection_3d/include/object_detection_3d/filter.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the Filter class. 3 | */ 4 | 5 | // TODO: Do we need a virtual destructor? 6 | 7 | #ifndef OBJECT_DETECTION_3D_FILTER_H 8 | #define OBJECT_DETECTION_3D_FILTER_H 9 | 10 | #include "pcl_types.h" 11 | 12 | 13 | namespace object_detection_3d { 14 | 15 | /** \brief Abstract filter for filtering a point cloud. */ 16 | class Filter { 17 | public: 18 | /** \brief Filters the given point cloud. 19 | * \param[in] cloud Point cloud that is filtered. 20 | * \return New point cloud that contains the filtered cloud. 21 | */ 22 | virtual PointCloudT::Ptr filter(const PointCloudT::ConstPtr& cloud) = 0; 23 | }; 24 | 25 | } // object_detection_3d 26 | 27 | #endif // OBJECT_DETECTION_3D_FILTER_H 28 | -------------------------------------------------------------------------------- /src/object_detection_3d/include/object_detection_3d/parameter_storage.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ParameterStorage class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_3D_PARAMETER_STORAGE_H 6 | #define OBJECT_DETECTION_3D_PARAMETER_STORAGE_H 7 | 8 | // std. 9 | #include 10 | #include 11 | 12 | // Our headers. 13 | #include "parametrizable.h" 14 | 15 | 16 | namespace object_detection_3d { 17 | 18 | /** \brief Class for handling the saving/loading of parameters 19 | * to/from parameter files. 20 | */ 21 | class ParameterStorage { 22 | using MultiClassParameterMap = Parametrizable::MultiClassParameterMap; 23 | using SingleClassParameterMap = Parametrizable::SingleClassParameterMap; 24 | 25 | public: 26 | /** \brief Registers a Parametrizable object to the internal vector of 27 | * references. 28 | */ 29 | void addParametrizable(Parametrizable*); 30 | 31 | /** \brief Saves the parameters of all registered Parametrizable objects 32 | * to a parameter file. 33 | * 34 | * The file with the given name is opened. 35 | * Then each registered Parametrizable is asked to provide a parameter-map 36 | * that contains all of its parameter-values, and those parameters are 37 | * written to the parameter file. 38 | * 39 | * \param[in] filename Name of the parameter-file. 40 | * \return true if the file has been created successfully, false otherwise. 41 | */ 42 | bool saveParameters(const std::string& filename) const; 43 | 44 | /** \brief Reads parameters from the given parameter-file, collects them 45 | * in a parameter-map, and passes the map to each registered Parametrizable. 46 | * \param[in] filename Name of the parameter-file. 47 | * \return true if the file with the given name has been opened successfully, 48 | * false otherwise. 49 | */ 50 | bool loadParameters(const std::string& filename) const; 51 | 52 | private: 53 | std::vector parametrizables_; 54 | }; 55 | 56 | } // object_detection_3d 57 | 58 | #endif // OBJECT_DETECTION_3D_PARAMETER_STORAGE_H 59 | -------------------------------------------------------------------------------- /src/object_detection_3d/include/object_detection_3d/parametrizable.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the Parametrizable class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_3D_PARAMETRIZABLE_H 6 | #define OBJECT_DETECTION_3D_PARAMETRIZABLE_H 7 | 8 | // std. 9 | #include 10 | #include 11 | 12 | 13 | namespace object_detection_3d { 14 | 15 | /** \brief Class for objects that have parameters and that are able to create a 16 | * parameter-map that contains the values of those parameters and 17 | * to extract those values from such a map. 18 | */ 19 | class Parametrizable { 20 | public: 21 | using SingleClassParameterMap = std::map; 22 | using MultiClassParameterMap = std::map; 23 | 24 | /** \brief Returns a parameter-map that contains the names and values 25 | * of all of the object's parameters. 26 | */ 27 | virtual MultiClassParameterMap createParameterMap() const = 0; 28 | 29 | /** \brief Reads the values of the parameters of the object from the 30 | * given parameter-map and sets the internal parameters accordingly. 31 | */ 32 | virtual void setParametersFromMap(const MultiClassParameterMap&) = 0; 33 | }; 34 | 35 | } // object_detection_3d 36 | 37 | #endif // OBJECT_DETECTION_3D_PARAMETRIZABLE_H 38 | -------------------------------------------------------------------------------- /src/object_detection_3d/include/object_detection_3d/pcl_types.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Common typedefs used for processing point clouds. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_3D_PCL_TYPES_H 6 | #define OBJECT_DETECTION_3D_PCL_TYPES_H 7 | 8 | #include // PointXYZ, Normal 9 | #include // PointCloud 10 | 11 | 12 | namespace object_detection_3d { 13 | 14 | using PointT = pcl::PointXYZ; 15 | using PointCloudT = pcl::PointCloud; 16 | 17 | using NormalT = pcl::Normal; 18 | using NormalCloudT = pcl::PointCloud; 19 | 20 | } // object_detection_3d 21 | 22 | #endif // OBJECT_DETECTION_3D_PCL_TYPES_H 23 | -------------------------------------------------------------------------------- /src/object_detection_3d/include/object_detection_3d/ros_detector_wrapper.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ROSDetectorWrapper class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_3D_ROS_DETECTOR_WRAPPER_H 6 | #define OBJECT_DETECTION_3D_ROS_DETECTOR_WRAPPER_H 7 | 8 | // ROS. 9 | #include 10 | 11 | // Our headers. 12 | #include "pcl_types.h" 13 | 14 | 15 | namespace object_detection_3d { 16 | 17 | class Detector; 18 | 19 | 20 | /** \brief ROS wrapper for objects of the Detector class. 21 | * 22 | * A ROSDetectorWrapper object integrates Detector objects into the ROS 23 | * framework by providing the necessary subscribers, publishers and event 24 | * handlers. 25 | */ 26 | class ROSDetectorWrapper { 27 | public: 28 | /** \brief Sets the internal Detector-reference and initializes 29 | * the ROS communication. 30 | * 31 | * \param[in] detector Valid pointer to the Detector object that is integrated 32 | * into the ROS framework. The ROSDetectorWrapper does not own this object. 33 | */ 34 | explicit ROSDetectorWrapper(Detector*); 35 | 36 | private: 37 | void cloudCallback(const PointCloudT::ConstPtr& cloud); 38 | 39 | // Data members. 40 | ros::NodeHandle nh_; 41 | ros::Subscriber cloud_sub_; 42 | ros::Publisher objects_pub_; 43 | 44 | Detector* detector_; 45 | }; 46 | 47 | } // object_detection_3d 48 | 49 | #endif // OBJECT_DETECTION_3D_ROS_DETECTOR_WRAPPER_H 50 | -------------------------------------------------------------------------------- /src/object_detection_3d/include/object_detection_3d/ros_filter_wrapper.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ROSFilterWrapper class. 3 | */ 4 | 5 | #ifndef OBJECT_DETECTION_3D_ROS_FILTER_WRAPPER_H 6 | #define OBJECT_DETECTION_3D_ROS_FILTER_WRAPPER_H 7 | 8 | // ROS. 9 | #include 10 | 11 | // Our headers. 12 | #include "pcl_types.h" 13 | 14 | 15 | namespace object_detection_3d { 16 | 17 | class Filter; 18 | 19 | 20 | /** \brief ROS wrapper for objects of the Filter class. 21 | * 22 | * A ROSFilterWrapper object integrates Filter objects into the ROS 23 | * framework by providing the necessary subscribers, publishers and event 24 | * handlers. 25 | */ 26 | class ROSFilterWrapper { 27 | public: 28 | /** \brief Sets the internal Filter-reference and initializes 29 | * the ROS communication. 30 | * 31 | * \param[in] filter Pointer to the Filter object that is integrated 32 | * into the ROS framework. The ROSFilterWrapper does not own this object. 33 | */ 34 | explicit ROSFilterWrapper(Filter* filter); 35 | 36 | private: 37 | void cloudCallback(const PointCloudT::ConstPtr& cloud); 38 | 39 | // Data members. 40 | ros::NodeHandle nh_; 41 | ros::Subscriber sub_; 42 | ros::Publisher pub_; 43 | 44 | Filter* filter_; 45 | }; 46 | 47 | } // object_detection_3d 48 | 49 | #endif // OBJECT_DETECTION_3D_ROS_FILTER_WRAPPER_H 50 | -------------------------------------------------------------------------------- /src/object_detection_3d/src/definition_director.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the DefinitionDirector class. 3 | */ 4 | 5 | // Our headers. 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using object_detection_3d_msgs::DetectedObject3DArray; 13 | 14 | 15 | namespace object_detection_3d { 16 | 17 | DefinitionDirector::DefinitionDirector() 18 | : gui_ {nullptr}, 19 | detector_ {nullptr} 20 | { 21 | subscriber_ = 22 | node_handle_.subscribe("input_points", 1, 23 | &DefinitionDirector::cloudCallback, this); 24 | } 25 | 26 | 27 | void DefinitionDirector::setGUI(GUI* gui) 28 | { 29 | gui_ = gui; 30 | } 31 | 32 | 33 | void DefinitionDirector::addFilter(Filter* filter) 34 | { 35 | if (filter) 36 | filters_.push_back(filter); 37 | } 38 | 39 | 40 | void DefinitionDirector::setDetector(Detector* detector) 41 | { 42 | detector_ = detector; 43 | } 44 | 45 | 46 | void DefinitionDirector::cloudCallback(const PointCloudT::ConstPtr& cloud) 47 | const 48 | { 49 | if (!gui_) 50 | return; 51 | 52 | PointCloudT::Ptr filtered_cloud {new PointCloudT}; 53 | *filtered_cloud = *cloud; 54 | for (auto filter_ptr : filters_) 55 | filtered_cloud = filter_ptr->filter(filtered_cloud); 56 | 57 | gui_->clearRenderWindow(); 58 | gui_->showCloud(cloud); 59 | gui_->showColoredCloud(filtered_cloud); 60 | 61 | if (detector_) { 62 | DetectedObject3DArray objects {detector_->detect(filtered_cloud)}; 63 | for (const auto& obj : objects.objects) 64 | gui_->showObject(obj); 65 | } 66 | } 67 | 68 | } // object_detection_3d 69 | -------------------------------------------------------------------------------- /src/object_detection_3d/src/detection_director.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the DetectionDirector class. 3 | */ 4 | 5 | // Our headers. 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using object_detection_3d_msgs::DetectedObject3DArray; 12 | 13 | 14 | namespace object_detection_3d { 15 | 16 | DetectionDirector::DetectionDirector() 17 | : detector_ {nullptr} 18 | { 19 | subscriber_ = 20 | node_handle_.subscribe("input_points", 1, 21 | &DetectionDirector::cloudCallback, this); 22 | publisher_ = 23 | node_handle_.advertise("detected_objects_3d", 10); 24 | } 25 | 26 | 27 | void DetectionDirector::addFilter(Filter* filter) 28 | { 29 | if (filter) 30 | filters_.push_back(filter); 31 | } 32 | 33 | 34 | void DetectionDirector::setDetector(Detector* detector) 35 | { 36 | detector_ = detector; 37 | } 38 | 39 | 40 | void DetectionDirector::cloudCallback( 41 | const PointCloudT::ConstPtr& cloud) const 42 | { 43 | if (!detector_) 44 | return; 45 | 46 | // Filter cloud. 47 | PointCloudT::Ptr filtered_cloud {new PointCloudT}; 48 | *filtered_cloud = *cloud; 49 | for (auto filter_ptr : filters_) 50 | filtered_cloud = filter_ptr->filter(filtered_cloud); 51 | 52 | // Detect objects and publish them. 53 | DetectedObject3DArray objects {detector_->detect(filtered_cloud)}; 54 | if (!objects.objects.empty()) 55 | publisher_.publish(objects); 56 | } 57 | 58 | } // object_detection_3d 59 | -------------------------------------------------------------------------------- /src/object_detection_3d/src/parameter_storage.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ParameterStorage class. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include 8 | #include 9 | 10 | // Our headers. 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | 16 | 17 | namespace object_detection_3d { 18 | 19 | void ParameterStorage::addParametrizable(Parametrizable* obj_ptr) 20 | { 21 | parametrizables_.push_back(obj_ptr); 22 | } 23 | 24 | 25 | bool ParameterStorage::saveParameters(const string& filename) const 26 | { 27 | ofstream ofs {filename}; 28 | if (!ofs) 29 | return false; 30 | 31 | for (auto obj : parametrizables_) { 32 | MultiClassParameterMap param_map { 33 | obj->createParameterMap() 34 | }; 35 | 36 | for (const auto& class_pair : param_map) { // normally just one class per obj 37 | const auto& current_class_map = class_pair.second; 38 | for (const auto& param_pair : current_class_map) 39 | ofs << class_pair.first << '/' // classname 40 | << param_pair.first << " : " // param-name 41 | << param_pair.second << '\n'; // param-value 42 | } 43 | 44 | const string seperator {"----"}; 45 | ofs << seperator << '\n'; 46 | } 47 | return true; 48 | } 49 | 50 | 51 | bool ParameterStorage::loadParameters(const string& filename) const 52 | { 53 | // Open file. 54 | ifstream ifs {filename}; 55 | if (!ifs) 56 | return false; 57 | 58 | // Read each line and create map from all parameters found. 59 | MultiClassParameterMap param_map; 60 | for (string line; getline(ifs, line); ) { 61 | regex pattern {R"(^\s*(\w+)\s*/\s*(\w+)\s*:\s*(.*)$)"}; 62 | // e.g. " DistanceFilter /min_distance_ : 0.5 " 63 | smatch matches; 64 | if (regex_match(line, matches, pattern)) 65 | param_map[matches[1]][matches[2]] = matches[3]; 66 | } 67 | 68 | // Give the map to all parametrizables_. 69 | for (Parametrizable* obj : parametrizables_) 70 | obj->setParametersFromMap(param_map); 71 | 72 | return true; 73 | } 74 | 75 | } // object_detection_3d 76 | -------------------------------------------------------------------------------- /src/object_detection_3d/src/ros_detector_wrapper.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ROSDetectorWrapper class. 3 | */ 4 | 5 | // std. 6 | #include // runtime_error 7 | 8 | // Our headers. 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | namespace object_detection_3d { 15 | 16 | ROSDetectorWrapper::ROSDetectorWrapper(Detector* det) 17 | : detector_ {det} 18 | { 19 | if (!det) 20 | throw std::runtime_error {"ROSDetectorWrapper: nullptr passed to constructor"}; 21 | 22 | cloud_sub_ = 23 | nh_.subscribe("input_points", 1, 24 | &ROSDetectorWrapper::cloudCallback, this); 25 | objects_pub_ = 26 | nh_.advertise( 27 | "detected_objects_3d", 10); 28 | } 29 | 30 | 31 | void ROSDetectorWrapper::cloudCallback(const PointCloudT::ConstPtr& cloud) 32 | { 33 | object_detection_3d_msgs::DetectedObject3DArray objects { 34 | detector_->detect(cloud)}; 35 | if (!objects.objects.empty()) 36 | objects_pub_.publish(objects); 37 | } 38 | 39 | } // object_detection_3d 40 | -------------------------------------------------------------------------------- /src/object_detection_3d/src/ros_filter_wrapper.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ROSFilterWrapper class. 3 | */ 4 | 5 | // std. 6 | #include // runtime_error 7 | 8 | // Our headers. 9 | #include 10 | #include 11 | 12 | 13 | namespace object_detection_3d { 14 | 15 | ROSFilterWrapper::ROSFilterWrapper(Filter* filter) 16 | : filter_ {filter} 17 | { 18 | if (!filter) 19 | throw std::runtime_error 20 | {"ROSFilterWrapper: nullptr passed to constructor"}; 21 | 22 | sub_ = nh_.subscribe("input_points", 1, 23 | &ROSFilterWrapper::cloudCallback, this); 24 | pub_ = nh_.advertise("output_points", 10); 25 | } 26 | 27 | 28 | void ROSFilterWrapper::cloudCallback(const PointCloudT::ConstPtr& cloud) 29 | { 30 | PointCloudT::Ptr filtered_cloud {filter_->filter(cloud)}; 31 | pub_.publish(filtered_cloud); 32 | } 33 | 34 | } // object_detection_3d 35 | -------------------------------------------------------------------------------- /src/object_detection_3d_msgs/msg/DetectedObject3D.msg: -------------------------------------------------------------------------------- 1 | string name 2 | OrientedBox box 3 | -------------------------------------------------------------------------------- /src/object_detection_3d_msgs/msg/DetectedObject3DArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | DetectedObject3D[] objects 3 | -------------------------------------------------------------------------------- /src/object_detection_3d_msgs/msg/OrientedBox.msg: -------------------------------------------------------------------------------- 1 | # Pose = position of centroid of object + orientation of object. 2 | geometry_msgs/Pose pose 3 | 4 | # Dimensions of the box. 5 | float64 width 6 | float64 height 7 | float64 depth 8 | -------------------------------------------------------------------------------- /src/object_detection_3d_nodes/launch/distance_filter_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/object_detection_3d_nodes/launch/distance_shape_definition.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/object_detection_3d_nodes/launch/distance_shape_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/object_detection_3d_nodes/launch/distance_shape_nodes_definition.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 16 | 17 | 21 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/object_detection_3d_nodes/launch/shape_detector_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/object_detection_3d_nodes/src/distance_filter_node.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief main function for the distance_filter node. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include 8 | 9 | // ROS. 10 | #include 11 | 12 | // Our headers. 13 | #include 14 | #include 15 | #include 16 | 17 | using object_detection_3d::ROSFilterWrapper; 18 | using distance_filter::DistanceFilter; 19 | using distance_filter::ReconfigureParameterManager; 20 | using namespace std; 21 | 22 | 23 | void setupAndRun(int argc, char** argv) 24 | { 25 | ros::init(argc, argv, "distance_filter"); 26 | 27 | ReconfigureParameterManager pm; 28 | DistanceFilter filter {&pm}; 29 | ROSFilterWrapper wrapper {&filter}; 30 | 31 | ros::spin(); 32 | } 33 | 34 | 35 | int main(int argc, char** argv) 36 | try { 37 | setupAndRun(argc, argv); 38 | return 0; 39 | } 40 | catch (const exception& e) { 41 | cerr << "std::exception: " << e.what() << '\n'; 42 | return 1; 43 | } 44 | catch (...) { 45 | cerr << "Unknown exception. Terminating program.\n"; 46 | return 1; 47 | } 48 | -------------------------------------------------------------------------------- /src/object_detection_3d_nodes/src/distance_shape_detection.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Node for detecting objects with the shape-detection system, 3 | * based on a given parameter-file. 4 | */ 5 | 6 | // std. 7 | #include 8 | #include 9 | 10 | // Our headers. 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using object_detection_3d::DetectionDirector; 21 | using object_detection_3d::ParameterStorage; 22 | using distance_filter::DistanceFilter; 23 | using shape_detector::ShapeDetector; 24 | 25 | 26 | void printUsage(const std::string& program_name) 27 | { 28 | std::cerr << "Usage: " << program_name << " \n"; 29 | } 30 | 31 | 32 | int main(int argc, char** argv) 33 | try { 34 | // Initialize ROS. 35 | ros::init(argc, argv, "object_detection"); 36 | 37 | // Check cmd-line args. 38 | if (argc != 2) { 39 | printUsage(argv[0]); 40 | return 0; 41 | } 42 | 43 | // Create distance-filter. 44 | distance_filter::ObservedParameterManager filter_pm; 45 | DistanceFilter filter {&filter_pm}; 46 | 47 | // Create shape-detector. 48 | shape_detector::ObservedParameterManager detector_pm; 49 | ShapeDetector detector {&detector_pm}; 50 | detector_pm.setObjectName("Garbage-Can"); 51 | // TODO: The object-name should be contained in the file. 52 | 53 | // Create parameter-storage. 54 | ParameterStorage storage; 55 | storage.addParametrizable(&filter_pm); 56 | storage.addParametrizable(&detector_pm); 57 | if (!storage.loadParameters(argv[1])) { 58 | std::cerr << "Cannot load parameters from file " << argv[1] << '\n'; 59 | return 1; 60 | } 61 | 62 | // Create detection-director. 63 | DetectionDirector director; 64 | director.addFilter(&filter); 65 | director.setDetector(&detector); 66 | 67 | // Event loop. 68 | ros::spin(); 69 | return 0; 70 | } 71 | catch (const std::exception& e) { 72 | std::cerr << "\nstd::exception: " << e.what() << "\n\n"; 73 | return 1; 74 | } 75 | catch (...) { 76 | std::cerr << "\nUnknown exception caught. Terminating program.\n\n"; 77 | return 1; 78 | } 79 | -------------------------------------------------------------------------------- /src/object_detection_3d_nodes/src/shape_detector_node.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief main function for the shape_detector node. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include 8 | 9 | // Our headers. 10 | #include 11 | #include 12 | #include 13 | 14 | using object_detection_3d::ROSDetectorWrapper; 15 | using shape_detector::ShapeDetector; 16 | using shape_detector::ReconfigureParameterManager; 17 | using namespace std; 18 | 19 | 20 | void setupAndRun(int argc, char** argv) 21 | { 22 | ros::init(argc, argv, "shape_detector"); 23 | 24 | ReconfigureParameterManager pm; 25 | ShapeDetector detector {&pm}; 26 | ROSDetectorWrapper wrapper {&detector}; 27 | 28 | ros::spin(); 29 | } 30 | 31 | 32 | int main(int argc, char** argv) 33 | try { 34 | setupAndRun(argc, argv); 35 | return 0; 36 | } 37 | catch (const std::exception& e) { 38 | cerr << "std::exception: " << e.what() << '\n'; 39 | return 1; 40 | } 41 | catch (...) { 42 | cerr << "Unknown exception. Terminating program.\n"; 43 | return 1; 44 | } 45 | -------------------------------------------------------------------------------- /src/object_painter/cfg/ObjectPainter.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "object_painter" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("max_age_s", double_t, 0, \ 9 | "Only objects up to this age (in seconds) are drawn", \ 10 | 0.3, 0., 10.) 11 | 12 | exit(gen.generate(PACKAGE, "object_painter", "ObjectPainter")) 13 | -------------------------------------------------------------------------------- /src/object_painter/include/object_painter/object_painter.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ObjectPainter class. 3 | */ 4 | 5 | #ifndef OBJECT_PAINTER_OBJECT_PAINTER_H 6 | #define OBJECT_PAINTER_OBJECT_PAINTER_H 7 | 8 | // std. 9 | #include 10 | 11 | // ROS. 12 | #include 13 | #include 14 | 15 | // Our packages. 16 | #include 17 | #include "parameter_manager.h" 18 | 19 | 20 | namespace object_painter { 21 | 22 | /** \brief The ObjectPainter draws 2D objects into images. 23 | * 24 | * The ObjectPainter subscribes to images and 2D detected objects. 25 | * When a new image is received, it draws the polygons representing the objects 26 | * into it and publishes the resulting image. 27 | * 28 | * Only objects up to an adjustable maximum age are retained and drawn into 29 | * images. 30 | */ 31 | class ObjectPainter { 32 | public: 33 | /** \brief Initializes the ROS communication. */ 34 | ObjectPainter(); 35 | 36 | private: 37 | void initROSCommunication(); 38 | 39 | void imageCallback(const sensor_msgs::Image::ConstPtr& image_msg); 40 | void processImageMsg(const sensor_msgs::Image::ConstPtr& image_msg); 41 | 42 | void objectsCallback( 43 | const object_detection_2d_msgs::DetectedObject2DArray::ConstPtr& objects); 44 | 45 | /** \brief Modify objects_ so that it only contains objects that are not older than 46 | * the max-age parameter. 47 | */ 48 | void updateObjects(); 49 | 50 | std::vector::iterator 51 | findBeginningOfYoungObjects(); 52 | 53 | // Data members. 54 | ros::NodeHandle nh_; 55 | ros::Subscriber image_sub_; 56 | ros::Subscriber objects_sub_; 57 | ros::Publisher image_pub_; 58 | 59 | ParameterManager param_manager_; 60 | std::vector 61 | objects_; 62 | // objects_ stores objects with descending age. 63 | // We assume that the time-stamp given in the header of each incoming 64 | // object-array is approximately equal to the moment of the message-arrival. 65 | }; 66 | 67 | } // object_painter 68 | 69 | #endif // OBJECT_PAINTER_OBJECT_PAINTER_H 70 | -------------------------------------------------------------------------------- /src/object_painter/include/object_painter/parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ParameterManager class. 3 | */ 4 | 5 | #ifndef OBJECT_PAINTER_PARAMETER_MANAGER_H 6 | #define OBJECT_PAINTER_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include // bind(), placeholders 10 | 11 | // ROS. 12 | #include // Duration 13 | #include 14 | 15 | // object_painter. 16 | #include 17 | 18 | 19 | namespace object_painter { 20 | 21 | /** \brief dynamic_reconfigure parameter-manager for ObjectPainter objects. 22 | * 23 | * A ParameterManager object manages the dynamic_reconfigure parameters of 24 | * an ObjectPainter object and provides getter-methods for accessing those 25 | * parameters. 26 | */ 27 | class ParameterManager { 28 | public: 29 | /** \brief Initializes the dynamic_reconfigure server and registers a callback 30 | * for this server. 31 | */ 32 | ParameterManager() 33 | { 34 | reconfigure_callback_ = std::bind(&ParameterManager::reconfigureCallback, this, 35 | std::placeholders::_1, std::placeholders::_2); 36 | reconfigure_server_.setCallback(reconfigure_callback_); 37 | } 38 | 39 | 40 | /** \brief Returns the value of the max-age parameter. */ 41 | ros::Duration maxAge() const { return max_age_; } 42 | 43 | private: 44 | void reconfigureCallback(object_painter::ObjectPainterConfig& config, 45 | uint32_t) 46 | { 47 | max_age_ = ros::Duration {config.max_age_s}; 48 | } 49 | 50 | // Data members. 51 | dynamic_reconfigure::Server 52 | reconfigure_server_; 53 | dynamic_reconfigure::Server:: 54 | CallbackType reconfigure_callback_; 55 | 56 | ros::Duration max_age_; 57 | }; 58 | 59 | } // object_painter 60 | 61 | #endif // OBJECT_PAINTER_PARAMETER_MANAGER_H 62 | -------------------------------------------------------------------------------- /src/object_painter/launch/object_painter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/object_painter/src/main.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief main function for the object_painter node. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include 8 | 9 | // object_painter. 10 | #include 11 | 12 | using object_painter::ObjectPainter; 13 | using namespace std; 14 | 15 | 16 | int main(int argc, char** argv) 17 | try { 18 | ros::init(argc, argv, "object_painter"); 19 | object_painter::ObjectPainter p; 20 | 21 | ros::spin(); 22 | return 0; 23 | } 24 | catch (const exception& e) { 25 | cerr << "std::exception: " << e.what() << '\n'; 26 | return 1; 27 | } 28 | catch (...) { 29 | cerr << "Unknown exception caught.\n"; 30 | return 1; 31 | } 32 | -------------------------------------------------------------------------------- /src/objects2d_to_objects3d/launch/objects2d_to_objects3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /src/objects_to_markers/cfg/Parameters.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "objects_to_markers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("lifetime_s", double_t, 0, 9 | "lifetime of published markers", 10 | 0.4, 0., 10.0) 11 | 12 | exit(gen.generate(PACKAGE, "objects_to_markers", "Parameters")) 13 | -------------------------------------------------------------------------------- /src/objects_to_markers/include/objects_to_markers/objects_to_markers.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ObjectsToMarkers class. 3 | */ 4 | 5 | #ifndef OBJECTS_TO_MARKERS_OBJECTS_TO_MARKERS_H 6 | #define OBJECTS_TO_MARKERS_OBJECTS_TO_MARKERS_H 7 | 8 | // std. 9 | #include 10 | 11 | // Our headers. 12 | #include 13 | 14 | #include "parameter_manager.h" 15 | 16 | 17 | namespace objects_to_markers { 18 | 19 | /** \brief Class for publishing object markers. 20 | * 21 | * ObjectsToMarkers objects subscribe to 3-dimensional detected objects 22 | * and publish Markers representing those objects. 23 | * 24 | * The markers are typically used for visualization in rviz. 25 | */ 26 | 27 | class ObjectsToMarkers { 28 | public: 29 | /** \brief Default constructor. 30 | * 31 | * Initializes the ROS communication. 32 | */ 33 | ObjectsToMarkers(); 34 | 35 | private: 36 | /** \brief Callback for incoming detected-objects-message. 37 | * 38 | * Receives incoming 3-dimensional objects and publishes markers representing 39 | * those objects. 40 | */ 41 | void objectsCallback( 42 | const object_detection_3d_msgs::DetectedObject3DArray::ConstPtr& objects) 43 | const; 44 | 45 | // Data members. 46 | ros::NodeHandle nh_; 47 | ros::Subscriber sub_; 48 | ros::Publisher pub_; 49 | 50 | ParameterManager param_manager_; 51 | }; 52 | 53 | } // objects_to_markers 54 | 55 | #endif // OBJECTS_TO_MARKERS_OBJECTS_TO_MARKERS_H 56 | -------------------------------------------------------------------------------- /src/objects_to_markers/include/objects_to_markers/parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ParameterManager class. 3 | */ 4 | 5 | #ifndef OBJECTS_TO_MARKERS_PARAMETER_MANAGER_H 6 | #define OBJECTS_TO_MARKERS_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include // bind(), placeholders 10 | 11 | // ROS. 12 | #include 13 | #include 14 | 15 | // Our headers. 16 | #include 17 | 18 | 19 | namespace objects_to_markers { 20 | 21 | /** \brief dynamic_reconfigure parameter-manager for ObjectsToMarkers objects. 22 | * 23 | * ParameterManager objects manage the dynamic_reconfigure parameters of 24 | * ObjectsToMarkers objects and provide getter-methods for accessing those 25 | * parameters. 26 | */ 27 | class ParameterManager { 28 | public: 29 | /** \brief Default constructor. 30 | * 31 | * Initializes the dynamic_reconfigure server and registers a callback 32 | * for this server. 33 | */ 34 | ParameterManager() 35 | { 36 | reconfigure_callback_ = std::bind( 37 | &ParameterManager::reconfigureCallback, this, 38 | std::placeholders::_1, std::placeholders::_2); 39 | reconfigure_server_.setCallback(reconfigure_callback_); 40 | } 41 | 42 | /** \brief Getter function for the 'lifetime' parameter. 43 | */ 44 | ros::Duration lifetime() const { return lifetime_; } 45 | 46 | private: 47 | void reconfigureCallback(ParametersConfig& config, uint32_t) 48 | { 49 | lifetime_ = ros::Duration{config.lifetime_s}; 50 | } 51 | 52 | // Data members. 53 | dynamic_reconfigure::Server reconfigure_server_; 54 | dynamic_reconfigure::Server::CallbackType reconfigure_callback_; 55 | 56 | ros::Duration lifetime_; 57 | }; 58 | 59 | } // objects_to_markers 60 | 61 | #endif // OBJECTS_TO_MARKERS_PARAMETER_MANAGER_H 62 | -------------------------------------------------------------------------------- /src/objects_to_markers/launch/objects_to_markers.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/objects_to_markers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | objects_to_markers 4 | 0.0.0 5 | The objects_to_markers package 6 | 7 | 8 | 9 | 10 | janosch 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | roscpp 45 | std_msgs 46 | visualization_msgs 47 | object_detection_3d_msgs 48 | dynamic_reconfigure 49 | 50 | roscpp 51 | std_msgs 52 | visualization_msgs 53 | object_detection_3d_msgs 54 | dynamic_reconfigure 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /src/objects_to_markers/src/main.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief main-function for creating an ObjectsToMarkers object and waiting 3 | * for ROS events. 4 | */ 5 | 6 | // std. 7 | #include 8 | #include 9 | 10 | // ROS & Co. 11 | #include 12 | #include 13 | 14 | using objects_to_markers::ObjectsToMarkers; 15 | using namespace std; 16 | 17 | 18 | void setupAndRun(int argc, char** argv) 19 | { 20 | ros::init(argc, argv, "objects_to_markers"); 21 | ObjectsToMarkers otm; 22 | 23 | ros::spin(); 24 | } 25 | 26 | 27 | int main(int argc, char** argv) 28 | try { 29 | setupAndRun(argc, argv); 30 | return 0; 31 | } 32 | catch (const exception& e) { 33 | cerr << "std::exception: " << e.what() << '\n'; 34 | return 1; 35 | } 36 | catch (...) { 37 | cerr << "Unknown exception. Terminating program.\n"; 38 | return 1; 39 | } 40 | -------------------------------------------------------------------------------- /src/objects_to_markers/src/marker_publisher_test.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Simple node to test how markers work. 3 | * 4 | * A test-marker is published in an infinite loop. 5 | * This allows for investigation of the marker, e.g. in rviz. 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | using visualization_msgs::Marker; 12 | 13 | 14 | Marker createTestMarker() 15 | { 16 | Marker m; 17 | 18 | m.type = Marker::CUBE; 19 | m.header.frame_id = "camera_depth_frame"; 20 | 21 | m.pose.position.x = 1; 22 | m.pose.position.y = 2; 23 | m.pose.position.z = 3; 24 | 25 | m.pose.orientation.x = 0; 26 | m.pose.orientation.y = 0; 27 | m.pose.orientation.z = 0.487864315758; 28 | m.pose.orientation.w = 0.872919474757; 29 | 30 | m.scale.x = 2; 31 | m.scale.y = 2; 32 | m.scale.z = 6; 33 | 34 | m.lifetime = ros::Duration{0.2}; 35 | 36 | m.color.r = 1; 37 | m.color.g = 0; 38 | m.color.b = 0; 39 | m.color.a = 1; 40 | 41 | return m; 42 | } 43 | 44 | 45 | int main(int argc, char** argv) 46 | { 47 | ros::init(argc, argv, "marker_publisher"); 48 | ros::NodeHandle nh; 49 | 50 | ros::Publisher pub {nh.advertise("markers", 10)}; 51 | 52 | Marker m {createTestMarker()}; 53 | 54 | ros::Rate rate {2}; 55 | while (ros::ok()) { 56 | pub.publish(m); 57 | rate.sleep(); 58 | ros::spinOnce(); 59 | } 60 | 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /src/objects_to_markers/src/objects_to_markers.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ObjectsToMarkers class. 3 | */ 4 | 5 | // ROS. 6 | #include 7 | 8 | // Msg types. 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | // Headers of this package. 18 | #include 19 | 20 | using std_msgs::Header; 21 | 22 | using visualization_msgs::Marker; 23 | using visualization_msgs::MarkerArray; 24 | 25 | using object_detection_3d_msgs::DetectedObject3D; 26 | using object_detection_3d_msgs::DetectedObject3DArray; 27 | 28 | 29 | namespace { 30 | 31 | /** \brief A MarkerBuilder builds a marker from an object. 32 | */ 33 | class MarkerBuilder { 34 | public: 35 | MarkerBuilder() 36 | { 37 | id_ = 0; 38 | } 39 | 40 | void setHeader(const Header& h) { header_ = h; } 41 | 42 | void setLifetime(const ros::Duration& lt) { lifetime_ = lt; } 43 | 44 | Marker buildMarkerFromObject(const DetectedObject3D& obj) 45 | { 46 | Marker m; 47 | 48 | m.header = header_; 49 | m.lifetime = lifetime_; 50 | m.type = Marker::CUBE; 51 | m.id = id_++; 52 | 53 | m.text = obj.name; 54 | 55 | m.pose = obj.box.pose; 56 | 57 | m.scale.x = obj.box.depth; 58 | m.scale.y = obj.box.width; // TODO: width or depth?? 59 | m.scale.z = obj.box.height; 60 | 61 | m.color.r = 0.; 62 | m.color.g = 1.; 63 | m.color.b = 0.; 64 | m.color.a = 0.5; 65 | 66 | return m; 67 | } 68 | 69 | private: 70 | Header header_; 71 | ros::Duration lifetime_; 72 | static int id_; 73 | }; 74 | 75 | int MarkerBuilder::id_ {0}; 76 | 77 | } // anonymous ns 78 | 79 | 80 | namespace objects_to_markers { 81 | 82 | ObjectsToMarkers::ObjectsToMarkers() 83 | { 84 | sub_ = nh_.subscribe("detected_objects_3d", 1, 85 | &ObjectsToMarkers::objectsCallback, this); 86 | pub_ = nh_.advertise("object_markers", 10); 87 | } 88 | 89 | 90 | void ObjectsToMarkers::objectsCallback( 91 | const DetectedObject3DArray::ConstPtr& objects) const 92 | { 93 | MarkerArray marker_array; 94 | 95 | MarkerBuilder builder; 96 | builder.setHeader(objects->header); 97 | builder.setLifetime(param_manager_.lifetime()); 98 | 99 | for (const auto& obj : objects->objects) { 100 | Marker m {builder.buildMarkerFromObject(obj)}; 101 | marker_array.markers.push_back(m); 102 | } 103 | 104 | pub_.publish(marker_array); 105 | } 106 | 107 | } // objects_to_markers 108 | -------------------------------------------------------------------------------- /src/shape_detector/cfg/Parameters.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "shape_detector" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("object_name", str_t, 0, \ 9 | "Name that is given to detected objects", "") 10 | 11 | shape_enum = gen.enum( \ 12 | [ \ 13 | gen.const("Cylinder", int_t, 5, "Detect a cylinder"), \ 14 | gen.const("Sphere", int_t, 4, "Detect a sphere") \ 15 | ], \ 16 | "An enum to set the shape") 17 | # The shape values have to be identical with the pcl::SACMODEL_* consts. 18 | gen.add("shape", int_t, 0, "Shape that we try to detect", 5, 4, 5, \ 19 | edit_method=shape_enum) 20 | 21 | RADIUS_LOWER_LIMIT = 0.01 22 | RADIUS_UPPER_LIMIT = 1.0 23 | radii = gen.add_group("Radii") 24 | radii.add("min_radius", double_t, 0, \ 25 | "Objects with a smaller radius will not be detected", \ 26 | 0.1, RADIUS_LOWER_LIMIT, RADIUS_UPPER_LIMIT) 27 | radii.add("max_radius", double_t, 0, \ 28 | "Objects with a greater radius will not be detected", \ 29 | 0.2, RADIUS_LOWER_LIMIT, RADIUS_UPPER_LIMIT) 30 | 31 | gen.add("normal_distance_weight", double_t, 0, \ 32 | "normal-distance-weight", \ 33 | 0.1, 0.001, 1.0) 34 | gen.add("distance_threshold", double_t, 0, \ 35 | "distance-threshold", \ 36 | 0.05, 0.001, 1.0) 37 | gen.add("num_nearest_neighbors", int_t, 0, \ 38 | "Number nearest neighbors used to calculate normals", \ 39 | 50, 5, 100) 40 | gen.add("max_iterations", int_t, 0, \ 41 | "Maximum number of iterations", \ 42 | 100, 1, 20000) 43 | gen.add("optimize_coefficients", bool_t, 0, \ 44 | "Optimize coefficients", \ 45 | True) 46 | 47 | exit(gen.generate(PACKAGE, "shape_detector", "Parameters")) 48 | -------------------------------------------------------------------------------- /src/shape_detector/include/shape_detector/groupbox.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the GroupBox class. 3 | */ 4 | 5 | #ifndef SHAPE_DETECTOR_GROUPBOX_H 6 | #define SHAPE_DETECTOR_GROUPBOX_H 7 | 8 | // TODO: Add widget for setting the object-name. 9 | 10 | // Qt. 11 | #include 12 | 13 | // Our headers. 14 | #include 15 | 16 | class QWidget; 17 | class QVBoxLayout; 18 | 19 | namespace shape_detector { 20 | 21 | class ObservedParameterManager; 22 | class NormalEstimationGroupBox; 23 | class SegmentationGroupBox; 24 | 25 | 26 | /** \brief GroupBox for accessing the parameters of a ShapeDetector. 27 | */ 28 | class GroupBox : public QGroupBox, 29 | public object_detection::Observer { 30 | Q_OBJECT 31 | 32 | public: 33 | /** \brief Creates and initializes a GroupBox. 34 | * 35 | * \param[in] parent Pointer to QWidget that serves as a parent of the 36 | * GroupBox. 37 | */ 38 | explicit GroupBox(QWidget* parent = nullptr); 39 | 40 | /** \brief Updates the view of the GroupBox so that it displays the current 41 | * values of the parameters. 42 | */ 43 | void update() override; 44 | 45 | /** \brief Sets the parameter-manager of the ShapeDetector. 46 | * 47 | * The ObservedParameterManager manages the parameters of a ShapeDetector 48 | * object. 49 | * The manager represents the Model of the MVC-pattern, while the GroupBox 50 | * represents the View and the Controller. 51 | */ 52 | void setParameterManager(ObservedParameterManager*); 53 | 54 | private: 55 | QVBoxLayout* createMainLayout() const; 56 | 57 | // Data members. 58 | NormalEstimationGroupBox* normal_estimation_groupbox_; 59 | SegmentationGroupBox* segmentation_groupbox_; 60 | }; 61 | 62 | } // shape_detector 63 | 64 | #endif // SHAPE_DETECTOR_GROUPBOX_H 65 | -------------------------------------------------------------------------------- /src/shape_detector/include/shape_detector/normal_estimation_groupbox.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the NormalEstimationGroupBox class. 3 | */ 4 | 5 | #ifndef SHAPE_DETECTOR_NORMAL_ESTIMATION_GROUPBOX_H 6 | #define SHAPE_DETECTOR_NORMAL_ESTIMATION_GROUPBOX_H 7 | 8 | // Qt. 9 | #include 10 | 11 | class QWidget; 12 | class QLabel; 13 | class QSpinBox; 14 | class QHBoxLayout; 15 | 16 | 17 | namespace shape_detector { 18 | 19 | class ObservedParameterManager; 20 | 21 | 22 | /** \brief QGroupBox for accessing the parameters of a ShapeDetector that are 23 | * related to the estimation of normals. 24 | */ 25 | class NormalEstimationGroupBox : public QGroupBox { 26 | Q_OBJECT 27 | 28 | public: 29 | /** \brief Creates and initializes the GroupBox. 30 | * 31 | * \param[in] parent Pointer to QWidget that serves as a parent of the 32 | * GroupBox. 33 | */ 34 | explicit NormalEstimationGroupBox(QWidget* parent = nullptr); 35 | 36 | /** \brief Updates the view of the GroupBox so that it displays the current 37 | * values of the parameters. 38 | */ 39 | void update() const; 40 | 41 | /** \brief Sets the parameter-manager of the ShapeDetector. 42 | * 43 | * The ObservedParameterManager manages the parameters of a ShapeDetector 44 | * object. 45 | * The manager represents the Model of the MVC-pattern, while the GroupBox 46 | * represents the View and the Controller. 47 | */ 48 | void setParameterManager(ObservedParameterManager* detector); 49 | 50 | private slots: 51 | void requestNumNearestNeighborsChange(int val) const; 52 | 53 | private: 54 | void createNearestNeighborsLabel(); 55 | void createNearestNeighborsSpinBox(); 56 | QHBoxLayout* createMainLayout() const; 57 | 58 | // Data members. 59 | ObservedParameterManager* param_manager_ {nullptr}; 60 | 61 | QLabel* nearest_neighbors_label_; 62 | QSpinBox* nearest_neighbors_spinbox_; 63 | }; 64 | 65 | } // shape_detector 66 | 67 | #endif // SHAPE_DETECTOR_NORMAL_ESTIMATION_GROUPBOX_H 68 | -------------------------------------------------------------------------------- /src/shape_detector/include/shape_detector/parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ParameterManager class. 3 | */ 4 | 5 | #ifndef SHAPE_DETECTOR_PARAMETER_MANAGER_H 6 | #define SHAPE_DETECTOR_PARAMETER_MANAGER_H 7 | 8 | // std. 9 | #include 10 | 11 | // Headers of this package. 12 | #include "shape_enum.h" 13 | 14 | 15 | namespace shape_detector { 16 | 17 | /** \brief Parameter-manager for ShapeDetector objects. 18 | * 19 | * ParameterManager objects manage the parameters of 20 | * ShapeDetector objects and provide getter- and setter-methods for accessing 21 | * those parameters. 22 | */ 23 | class ParameterManager { 24 | public: 25 | // static consts. 26 | static constexpr double radius_lower_limit {0.01}; 27 | static constexpr double radius_upper_limit {1.}; 28 | 29 | static constexpr double normal_distance_weight_lower_limit {0.001}; 30 | static constexpr double normal_distance_weight_upper_limit {1.}; 31 | 32 | static constexpr double distance_threshold_lower_limit {0.05}; 33 | static constexpr double distance_threshold_upper_limit {1.}; 34 | 35 | static constexpr int num_nearest_neighbors_lower_limit {5}; 36 | static constexpr int num_nearest_neighbors_upper_limit {100}; 37 | 38 | static constexpr int max_iterations_lower_limit {1}; 39 | static constexpr int max_iterations_upper_limit {20000}; 40 | 41 | // Getters & Setters. 42 | std::string objectName() const { return object_name_; } 43 | void setObjectName(const std::string&); 44 | 45 | Shape shape() const { return shape_; } 46 | void setShape(Shape); 47 | 48 | double minRadius() const { return min_radius_; } 49 | void setMinRadius(double); 50 | 51 | double maxRadius() const { return max_radius_; } 52 | void setMaxRadius(double); 53 | 54 | double normalDistanceWeight() const { return normal_distance_weight_; } 55 | void setNormalDistanceWeight(double); 56 | 57 | double distanceThreshold() const { return distance_threshold_; } 58 | void setDistanceThreshold(double); 59 | 60 | int numNearestNeighbors() const { return num_nearest_neighbors_; } 61 | void setNumNearestNeighbors(int); 62 | 63 | int maxIterations() const { return max_iterations_; } 64 | void setMaxIterations(int); 65 | 66 | bool optimizeCoefficients() const { return optimize_coefficients_; } 67 | void setOptimizeCoefficients(bool); 68 | 69 | private: 70 | // Parameters. 71 | std::string object_name_; 72 | 73 | Shape shape_ {Shape::cylinder}; 74 | double min_radius_ {0.1}; 75 | double max_radius_ {0.2}; 76 | double normal_distance_weight_ {0.1}; 77 | double distance_threshold_ {0.05}; 78 | int num_nearest_neighbors_{50}; 79 | int max_iterations_ {100}; 80 | bool optimize_coefficients_ {true}; 81 | }; 82 | 83 | } // shape_detector 84 | 85 | #endif // SHAPE_DETECTOR_PARAMETER_MANAGER_H 86 | -------------------------------------------------------------------------------- /src/shape_detector/include/shape_detector/reconfigure_parameter_manager.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ReconfigureParameterManager class. 3 | */ 4 | 5 | #ifndef SHAPE_DETECTOR_RECONFIGURE_PARAMETER_MANAGER_H 6 | #define SHAPE_DETECTOR_RECONFIGURE_PARAMETER_MANAGER_H 7 | 8 | // dynamic_reconfigure. 9 | #include 10 | #include 11 | 12 | // Headers of this package. 13 | #include "shape_enum.h" 14 | #include "parameter_manager.h" 15 | 16 | 17 | namespace shape_detector { 18 | 19 | /** \brief dynamic_reconfigure parameter-manager for ShapeDetector objects. 20 | * 21 | * A ReconfigureParameterManager object allows to manage the parameters 22 | * of a ShapeDetector object via a corresponding set of dynamic_reconfigure 23 | * parameters. 24 | */ 25 | class ReconfigureParameterManager : public ParameterManager { 26 | public: 27 | /** \brief Initializes the dynamic_reconfigure server and registers a callback 28 | * for this server. 29 | */ 30 | ReconfigureParameterManager(); 31 | 32 | private: 33 | void reconfigureCallback(shape_detector::ParametersConfig& config, 34 | uint32_t); 35 | 36 | // dynamic_reconfigure server & callback. 37 | dynamic_reconfigure::Server reconfigure_server_; 38 | dynamic_reconfigure::Server::CallbackType 39 | reconfigure_callback_; 40 | }; 41 | 42 | } // shape_detector 43 | 44 | #endif // SHAPE_DETECTOR_RECONFIGURE_PARAMETER_MANAGER_H 45 | -------------------------------------------------------------------------------- /src/shape_detector/include/shape_detector/shape_detector.h: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Definition of the ShapeDetector class. 3 | */ 4 | 5 | #ifndef SHAPE_DETECTOR_SHAPE_DETECTOR_H 6 | #define SHAPE_DETECTOR_SHAPE_DETECTOR_H 7 | 8 | // std. 9 | #include // pair 10 | 11 | // PCL. 12 | #include 13 | #include 14 | #include 15 | 16 | // object_detection_*. 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | // Headers of this package. 23 | #include "shape_enum.h" 24 | 25 | 26 | namespace shape_detector { 27 | 28 | class ParameterManager; 29 | 30 | 31 | // TODO: This is probably not the right place for those using's. 32 | using PointT = object_detection_3d::PointT; 33 | using PointCloudT = object_detection_3d::PointCloudT; 34 | 35 | using NormalT = object_detection_3d::NormalT; 36 | using NormalCloudT = object_detection_3d::NormalCloudT; 37 | 38 | 39 | /** \brief Detects 3-dimensional shapes. 40 | */ 41 | class ShapeDetector : public object_detection_3d::Detector { 42 | public: 43 | /** \brief Creates a ShapeDetector and initializes its parameter-manager. 44 | * 45 | * \param[in] Pointer to a valid ParameterManager. 46 | */ 47 | explicit ShapeDetector(ParameterManager* pm); 48 | 49 | /** \brief Tries to detect a 3-dimensional shape. 50 | * 51 | * \note The parameters (including the shape) of this algorithm are managed 52 | * by the ParameterManager data member. 53 | * 54 | * \return An array of objects. This array contains one object if the shape 55 | * was found; it contains zero objects otherwise. 56 | * 57 | * \note Even though an array of 3-dimensional objects is returned, this 58 | * array can contain at most one object. This is done to adhere to the 59 | * signature used by all Detectors. 60 | */ 61 | object_detection_3d_msgs::DetectedObject3DArray detect( 62 | const PointCloudT::ConstPtr& cloud) override; 63 | 64 | private: 65 | std::pair tryDetectObject( 66 | const PointCloudT::ConstPtr& cloud); 67 | 68 | std::pair 69 | segmentShapeFromCloud(const PointCloudT::ConstPtr& cloud, 70 | const NormalCloudT::ConstPtr& normals); 71 | 72 | pcl::SACSegmentationFromNormals 73 | createSACSegmentation() const; 74 | 75 | // Data members. 76 | ParameterManager* param_manager_; 77 | }; 78 | 79 | } // shape_detector 80 | 81 | #endif // SHAPE_DETECTOR_SHAPE_DETECTOR_H 82 | -------------------------------------------------------------------------------- /src/shape_detector/include/shape_detector/shape_enum.h: -------------------------------------------------------------------------------- 1 | /** \file shape_enum.h 2 | * \brief Definition of the Shape enumeration. 3 | */ 4 | 5 | #ifndef SHAPE_DETECTOR_SHAPE_ENUM_H 6 | #define SHAPE_DETECTOR_SHAPE_ENUM_H 7 | 8 | #include 9 | 10 | 11 | namespace shape_detector { 12 | 13 | /** \brief Enumeration for shapes that can be detected by the ShapeDetector. 14 | */ 15 | enum class Shape { 16 | sphere = pcl::SACMODEL_SPHERE, 17 | cylinder = pcl::SACMODEL_CYLINDER 18 | }; 19 | 20 | } // shape_detector 21 | 22 | #endif // SHAPE_DETECTOR_SHAPE_ENUM_H 23 | -------------------------------------------------------------------------------- /src/shape_detector/rename.bash: -------------------------------------------------------------------------------- 1 | # Check argc. 2 | case $# in 3 | 2) ;; 4 | *) echo "Usage: $0 "; exit 1;; 5 | esac 6 | 7 | from=$1 8 | to=$2 9 | cap_from=`echo $from | sed 's/[a-z]/\u&/g'` 10 | cap_to=`echo $to | sed 's/[a-z]/\u&/g'` 11 | 12 | # Rename headers. 13 | for i in `ls include/$to` 14 | do 15 | sed 's/'$cap_from'/'$cap_to'/g 16 | s/'$from'/'$to'/g' include/$to/$i >include/$to/new_$i 17 | mv include/$to/new_$i include/$to/$i 18 | done 19 | 20 | # Rename source files. 21 | for i in `ls src` 22 | do 23 | sed 's/'$cap_from'/'$cap_to'/g 24 | s/'$from'/'$to'/g' src/$i >src/new_$i 25 | mv src/new_$i src/$i 26 | done 27 | -------------------------------------------------------------------------------- /src/shape_detector/src/groupbox.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the GroupBox class. 3 | */ 4 | 5 | // Qt. 6 | #include 7 | #include 8 | #include 9 | 10 | // Headers of this package. 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | namespace shape_detector { 18 | 19 | GroupBox::GroupBox(QWidget* parent) 20 | : QGroupBox {"Shape Detector", parent}, 21 | normal_estimation_groupbox_ {new NormalEstimationGroupBox {this}}, 22 | segmentation_groupbox_ {new SegmentationGroupBox {this}} 23 | { 24 | QVBoxLayout* main_layout {createMainLayout()}; 25 | setLayout(main_layout); 26 | } 27 | 28 | 29 | QVBoxLayout* GroupBox::createMainLayout() const 30 | { 31 | QVBoxLayout* layout {new QVBoxLayout}; 32 | layout->addWidget(normal_estimation_groupbox_); 33 | layout->addWidget(segmentation_groupbox_); 34 | 35 | return layout; 36 | } 37 | 38 | 39 | void GroupBox::update() 40 | { 41 | normal_estimation_groupbox_->update(); 42 | segmentation_groupbox_->update(); 43 | } 44 | 45 | 46 | void GroupBox::setParameterManager(ObservedParameterManager* pm) 47 | { 48 | normal_estimation_groupbox_->setParameterManager(pm); 49 | segmentation_groupbox_->setParameterManager(pm); 50 | } 51 | 52 | } // shape_detector 53 | -------------------------------------------------------------------------------- /src/shape_detector/src/normal_estimation_groupbox.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the NormalEstimationGroupBox class. 3 | */ 4 | 5 | // Qt. 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // Headers of this package. 13 | #include 14 | #include 15 | 16 | 17 | namespace shape_detector { 18 | 19 | NormalEstimationGroupBox::NormalEstimationGroupBox(QWidget* parent) 20 | : QGroupBox {"&Normal Estimation", parent}, 21 | param_manager_ {nullptr} 22 | { 23 | createNearestNeighborsLabel(); 24 | createNearestNeighborsSpinBox(); 25 | QHBoxLayout* layout {createMainLayout()}; 26 | setLayout(layout); 27 | } 28 | 29 | 30 | void NormalEstimationGroupBox::createNearestNeighborsLabel() 31 | { 32 | nearest_neighbors_label_ = new QLabel {"Nearest Neighbors:"}; 33 | } 34 | 35 | 36 | void NormalEstimationGroupBox::createNearestNeighborsSpinBox() 37 | { 38 | nearest_neighbors_spinbox_ = new QSpinBox; 39 | 40 | nearest_neighbors_spinbox_->setRange( 41 | ObservedParameterManager::num_nearest_neighbors_lower_limit, 42 | ObservedParameterManager::num_nearest_neighbors_upper_limit 43 | ); 44 | 45 | connect( 46 | nearest_neighbors_spinbox_, 47 | static_cast(&QSpinBox::valueChanged), 48 | this, &NormalEstimationGroupBox::requestNumNearestNeighborsChange 49 | ); 50 | } 51 | 52 | 53 | QHBoxLayout* NormalEstimationGroupBox::createMainLayout() const 54 | { 55 | QHBoxLayout* layout {new QHBoxLayout}; 56 | 57 | layout->addWidget(nearest_neighbors_label_); 58 | layout->addWidget(nearest_neighbors_spinbox_); 59 | 60 | return layout; 61 | } 62 | 63 | 64 | void NormalEstimationGroupBox::update() const 65 | { 66 | if (param_manager_) 67 | nearest_neighbors_spinbox_->setValue( 68 | param_manager_->numNearestNeighbors() 69 | ); 70 | } 71 | 72 | 73 | void NormalEstimationGroupBox::setParameterManager(ObservedParameterManager* pm) 74 | { 75 | param_manager_ = pm; 76 | update(); 77 | } 78 | 79 | 80 | void NormalEstimationGroupBox::requestNumNearestNeighborsChange(int val) const 81 | { 82 | if (param_manager_) 83 | param_manager_->setNumNearestNeighbors(val); 84 | } 85 | 86 | } // shape_detector 87 | -------------------------------------------------------------------------------- /src/shape_detector/src/object_computer.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of general ObjectComputer-related functionality. 3 | */ 4 | 5 | // std. 6 | #include // forward 7 | #include // unique_ptr 8 | #include // runtime_error 9 | 10 | // Headers of this package. 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | 16 | 17 | namespace { 18 | 19 | template 20 | unique_ptr make_unique(Args&&... args) 21 | { 22 | return unique_ptr{new T{forward(args)...}}; 23 | } // just because it isn't available in C++11 24 | 25 | } // anonymous ns 26 | 27 | 28 | namespace shape_detector { 29 | 30 | unique_ptr createObjectComputerForShape(Shape shape) 31 | { 32 | switch (shape) { 33 | case Shape::cylinder: 34 | return make_unique(); 35 | case Shape::sphere: 36 | return make_unique(); 37 | } 38 | 39 | throw runtime_error {"createObjectComputerForShape: invalid shape"}; 40 | } 41 | 42 | } // shape_detector 43 | -------------------------------------------------------------------------------- /src/shape_detector/src/parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ParameterManager class. 3 | */ 4 | 5 | // std. 6 | #include 7 | #include // min(), max() 8 | 9 | // Headers of this package. 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | 15 | 16 | namespace { 17 | 18 | template 19 | T truncateToRange(T val, T lower_limit, T upper_limit) 20 | { 21 | return min(max(val, lower_limit), upper_limit); 22 | } 23 | 24 | } // anonymous ns 25 | 26 | 27 | namespace shape_detector { 28 | 29 | void ParameterManager::setObjectName(const string& name) 30 | { 31 | object_name_ = name; 32 | } 33 | 34 | 35 | void ParameterManager::setShape(Shape s) 36 | { 37 | switch (s) { 38 | case Shape::sphere: 39 | case Shape::cylinder: 40 | break; 41 | default: 42 | throw runtime_error {"ParameterManager::setShape: invalid argument"}; 43 | } 44 | 45 | shape_ = s; 46 | } 47 | 48 | 49 | void ParameterManager::setMinRadius(double val) 50 | { 51 | min_radius_ = truncateToRange(val, radius_lower_limit, radius_upper_limit); 52 | } 53 | 54 | 55 | void ParameterManager::setMaxRadius(double val) 56 | { 57 | max_radius_ = truncateToRange(val, radius_lower_limit, radius_upper_limit); 58 | } 59 | 60 | 61 | void ParameterManager::setNormalDistanceWeight(double val) 62 | { 63 | normal_distance_weight_ = 64 | truncateToRange(val, normal_distance_weight_lower_limit, 65 | normal_distance_weight_upper_limit); 66 | } 67 | 68 | 69 | void ParameterManager::setDistanceThreshold(double val) 70 | { 71 | distance_threshold_ = truncateToRange(val, distance_threshold_lower_limit, 72 | distance_threshold_upper_limit); 73 | } 74 | 75 | 76 | void ParameterManager::setNumNearestNeighbors(int val) 77 | { 78 | num_nearest_neighbors_ = truncateToRange(val, num_nearest_neighbors_lower_limit, 79 | num_nearest_neighbors_upper_limit); 80 | } 81 | 82 | 83 | void ParameterManager::setMaxIterations(int val) 84 | { 85 | max_iterations_ = truncateToRange(val, max_iterations_lower_limit, 86 | max_iterations_upper_limit); 87 | } 88 | 89 | 90 | void ParameterManager::setOptimizeCoefficients(bool val) 91 | { 92 | optimize_coefficients_ = val; 93 | } 94 | 95 | } // shape_detector 96 | -------------------------------------------------------------------------------- /src/shape_detector/src/reconfigure_parameter_manager.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the ReconfigureParameterManager class. 3 | */ 4 | 5 | // std. 6 | #include // bind(), placeholders 7 | #include // max() 8 | 9 | // Headers of this package. 10 | #include 11 | #include 12 | 13 | 14 | namespace shape_detector { 15 | 16 | ReconfigureParameterManager::ReconfigureParameterManager() 17 | { 18 | reconfigure_callback_ = std::bind(&ReconfigureParameterManager::reconfigureCallback, 19 | this, 20 | std::placeholders::_1, std::placeholders::_2); 21 | reconfigure_server_.setCallback(reconfigure_callback_); 22 | } 23 | 24 | void ReconfigureParameterManager::reconfigureCallback( 25 | shape_detector::ParametersConfig& config, uint32_t) 26 | { 27 | setObjectName(config.object_name); 28 | setShape(static_cast(config.shape)); 29 | 30 | // Groups do not work yet. 31 | // That's why we cannot write 'config.groups.Radii.min_radius'. 32 | config.max_radius = std::max(config.min_radius, config.max_radius); 33 | setMinRadius(config.min_radius); 34 | setMaxRadius(config.max_radius); 35 | 36 | setNormalDistanceWeight(config.normal_distance_weight); 37 | setDistanceThreshold(config.distance_threshold); 38 | setNumNearestNeighbors(config.num_nearest_neighbors); 39 | setMaxIterations(config.max_iterations); 40 | setOptimizeCoefficients(config.optimize_coefficients); 41 | } 42 | 43 | } // shape_detector 44 | -------------------------------------------------------------------------------- /src/shape_detector/src/sphere_computer.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Implementation of the SphereComputer class. 3 | */ 4 | 5 | // ROS. 6 | #include 7 | #include 8 | 9 | // object_detection_*. 10 | #include 11 | 12 | // Headers of this package. 13 | #include 14 | 15 | using object_detection_3d_msgs::DetectedObject3D; 16 | using namespace std; 17 | 18 | 19 | namespace shape_detector { 20 | 21 | DetectedObject3D SphereComputer::computeObject() 22 | { 23 | DetectedObject3D obj; 24 | 25 | obj.box.pose.position = computePosition(); 26 | obj.box.pose.orientation = computeOrientation(); 27 | obj.box.width = obj.box.height = obj.box.depth = computeDiameter(); 28 | 29 | return obj; 30 | } 31 | 32 | 33 | geometry_msgs::Point SphereComputer::computePosition() const 34 | { 35 | geometry_msgs::Point position; 36 | 37 | position.x = model_coefficients_->values[0]; 38 | position.y = model_coefficients_->values[1]; 39 | position.z = model_coefficients_->values[2]; 40 | 41 | return position; 42 | } 43 | 44 | 45 | geometry_msgs::Quaternion SphereComputer::computeOrientation() const 46 | { 47 | geometry_msgs::Quaternion q; 48 | 49 | q.x = 0; 50 | q.y = 0; 51 | q.z = 0; 52 | q.w = 1; 53 | 54 | return q; 55 | } 56 | 57 | 58 | double SphereComputer::computeDiameter() const 59 | { 60 | return 2 * model_coefficients_->values[3]; 61 | } 62 | 63 | } // shape_detector 64 | --------------------------------------------------------------------------------