From 70236428f722dab513eada4317b4e505e9ec8be7 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 20 Apr 2019 07:54:31 +0000 Subject: [PATCH 1/6] support catkin_lint and clang-format tests --- .clang-format | 66 +++++++++++++++++++ .clang-tidy | 50 +++++++++++++++ .travis.sh | 172 ++++++++++++++++++++++++++++++++++---------------- .travis.yml | 5 +- 4 files changed, 239 insertions(+), 54 deletions(-) create mode 100644 .clang-format create mode 100644 .clang-tidy mode change 100755 => 100644 .travis.sh diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..bef33267 --- /dev/null +++ b/.clang-format @@ -0,0 +1,66 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortLoopsOnASingleLine: false +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 100 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 70 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 00000000..7fae3668 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,50 @@ +--- +Checks: '-*, + performance-*, + llvm-namespace-comment, + modernize-redundant-void-arg, + modernize-use-nullptr, + modernize-use-default, + modernize-use-override, + modernize-loop-convert, + readability-named-parameter, + readability-redundant-smartptr-get, + readability-redundant-string-cstr, + readability-simplify-boolean-expr, + readability-container-size-empty, + readability-identifier-naming, + ' +HeaderFilterRegex: '' +AnalyzeTemporaryDtors: false +CheckOptions: + - key: llvm-namespace-comment.ShortNamespaceLines + value: '10' + - key: llvm-namespace-comment.SpacesBeforeComments + value: '2' + - key: readability-braces-around-statements.ShortStatementLines + value: '2' + # type names + - key: readability-identifier-naming.ClassCase + value: CamelCase + - key: readability-identifier-naming.EnumCase + value: CamelCase + - key: readability-identifier-naming.UnionCase + value: CamelCase + # method names + - key: readability-identifier-naming.MethodCase + value: camelBack + # variable names + - key: readability-identifier-naming.VariableCase + value: lower_case + - key: readability-identifier-naming.ClassMemberSuffix + value: '_' + # const static or global variables are UPPER_CASE + - key: readability-identifier-naming.EnumConstantCase + value: UPPER_CASE + - key: readability-identifier-naming.StaticConstantCase + value: UPPER_CASE + - key: readability-identifier-naming.ClassConstantCase + value: UPPER_CASE + - key: readability-identifier-naming.GlobalVariableCase + value: UPPER_CASE +... diff --git a/.travis.sh b/.travis.sh old mode 100755 new mode 100644 index d37b5c2b..9967b6d1 --- a/.travis.sh +++ b/.travis.sh @@ -22,65 +22,131 @@ function travis_time_end { set -x } +function setup { + travis_time_start setup.before_install + #before_install: + # Install ROS + sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main\" > /etc/apt/sources.list.d/ros-latest.list" + wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + sudo apt-get update -qq + # Install ROS + sudo apt-get install -y -q python-catkin-pkg python-catkin-tools python-rosdep python-wstool python-rosinstall-generator ros-$ROS_DISTRO-catkin + source /opt/ros/$ROS_DISTRO/setup.bash + # Setup for rosdep + sudo rosdep init + rosdep update + travis_time_end + + travis_time_start setup.install + #install: + mkdir -p ~/catkin_ws/src + + # Add the package under test to the workspace. + cd ~/catkin_ws/src + ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace + + # Install all dependencies, using wstool and rosdep. + # wstool looks for a ROSINSTALL_FILE defined in before_install. + travis_time_end + + travis_time_start setup.before_script + #before_script: + # source dependencies: install using wstool. + cd ~/catkin_ws/src + wstool init + #if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi + if [ "$OPENCV_VERSION" == 3 ]; then rosinstall_generator image_pipeline --upstream >> .rosinstall.opencv3; fi # need to recompile image_proc + if [ "$OPENCV_VERSION" == 3 ]; then rosinstall_generator compressed_image_transport --upstream >> .rosinstall.opencv3; fi # need to recompile compressed_image_transport + if [ "$OPENCV_VERSION" == 3 ]; then rosinstall_generator vision_opencv --upstream >> .rosinstall.opencv3; fi # need to recompile visoin_opencv + if [ "$OPENCV_VERSION" == 3 ]; then wstool merge .rosinstall.opencv3; fi # need to recompile visoin_opencv + wstool up + wstool info + if [ "$OPENCV_VERSION" == 3 ]; then sed -i 's@libopencv-dev@opencv3@' */*/package.xml ; fi + + + # package depdencies: install using rosdep. + cd ~/catkin_ws + rosdep install -q -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO + travis_time_end +} + +function build { + travis_time_start build.script + source /opt/ros/$ROS_DISTRO/setup.bash + cd ~/catkin_ws + catkin build -p1 -j1 --no-status + travis_time_end +} + +function run_test { + travis_time_start run_test.script + source /opt/ros/$ROS_DISTRO/setup.bash + cd ~/catkin_ws + catkin run_tests -p1 -j1 --no-status opencv_apps --no-deps + catkin_test_results --verbose build || catkin_test_results --all build + travis_time_end +} + +function build_install { + travis_time_start build_install.script + source /opt/ros/$ROS_DISTRO/setup.bash + cd ~/catkin_ws + catkin clean -b --yes || catkin clean -b -a + catkin config --install + catkin build -p1 -j1 --no-status + travis_time_end +} + +travis_time_start apt.before_install apt-get update -qq && apt-get install -y -q wget sudo lsb-release gnupg # for docker # set DEBIAN_FRONTEND=noninteractive echo 'debconf debconf/frontend select Noninteractive' | sudo debconf-set-selections - -travis_time_start setup.before_install -#before_install: -# Install ROS -sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main\" > /etc/apt/sources.list.d/ros-latest.list" -wget http://packages.ros.org/ros.key -O - | sudo apt-key add - -sudo apt-get update -qq -# Install ROS -sudo apt-get install -y -q python-catkin-pkg python-catkin-tools python-rosdep python-wstool python-rosinstall-generator ros-$ROS_DISTRO-catkin -source /opt/ros/$ROS_DISTRO/setup.bash -# Setup for rosdep -sudo rosdep init -rosdep update travis_time_end -travis_time_start setup.install -#install: -mkdir -p ~/catkin_ws/src +if [ $TEST == "catkin_lint" ]; then -# Add the package under test to the workspace. -cd ~/catkin_ws/src -ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace + travis_time_start catkin_lint.script + apt-get install -y -q python-pip + pip install catkin_lint rosdep + rosdep init + rosdep update + travis_time_end + ROS_DISTRO=melodic catkin_lint --resolve-env --strict $CI_SOURCE_PATH -# Install all dependencies, using wstool and rosdep. -# wstool looks for a ROSINSTALL_FILE defined in before_install. -travis_time_end -travis_time_start setup.before_script -#before_script: -# source dependencies: install using wstool. -cd ~/catkin_ws/src -wstool init -#if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi -if [ $OPENCV_VERSION == 3 ]; then rosinstall_generator image_pipeline --upstream >> .rosinstall.opencv3; fi # need to recompile image_proc -if [ $OPENCV_VERSION == 3 ]; then rosinstall_generator compressed_image_transport --upstream >> .rosinstall.opencv3; fi # need to recompile compressed_image_transport -if [ $OPENCV_VERSION == 3 ]; then rosinstall_generator vision_opencv --upstream >> .rosinstall.opencv3; fi # need to recompile visoin_opencv -if [ $OPENCV_VERSION == 3 ]; then wstool merge .rosinstall.opencv3; fi # need to recompile visoin_opencv -wstool up -wstool info -if [ $OPENCV_VERSION == 3 ]; then sed -i 's@libopencv-dev@opencv3@' */*/package.xml ; fi - - -# package depdencies: install using rosdep. -cd ~/catkin_ws -rosdep install -q -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO -travis_time_end +elif [ $TEST == "clang-format" ]; then -# Compile and test. -#script: -travis_time_start setup.script -source /opt/ros/$ROS_DISTRO/setup.bash -cd ~/catkin_ws -catkin build -p1 -j1 --no-status -catkin run_tests -p1 -j1 --no-status opencv_apps --no-deps -catkin_test_results --verbose build || catkin_test_results --all build -catkin clean -b --yes || catkin clean -b -a -catkin config --install -catkin build -p1 -j1 --no-status -travis_time_end + travis_time_start clang_format.script + apt-get install -y -q clang-format-3.9 git + find $CI_SOURCE_PATH -name '*.h' -or -name '*.hpp' -or -name '*.cpp' | xargs clang-format-3.9 -i -style=file + travis_time_end + git -C $CI_SOURCE_PATH --no-pager diff + git -C $CI_SOURCE_PATH diff-index --quiet HEAD -- . + +elif [ $TEST == "clang-tidy" ]; then + + setup + + travis_time_start clang_tidy.script + apt-get install -y -q clang-tidy clang-tools + cd ~/catkin_ws + catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON + travis_time_end + + build + + travis_time_start clang_tidy.script + for file in $(find ~/catkin_ws/build -name compile_commands.json) ; do + run-clang-tidy -fix -p $(dirname $file) + done + travis_time_end + git -C $CI_SOURCE_PATH --no-pager diff + git -C $CI_SOURCE_PATH diff-index --quiet HEAD -- . + +else + # Compile and test. + setup + build + run_test + build_install +fi diff --git a/.travis.yml b/.travis.yml index 19b19b53..f5b0d711 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,6 +2,9 @@ sudo: required dist: trusty language: generic env: + - TEST=catkin_lint DOCKER_IMAGE=ubuntu:bionic + - TEST=clang-format DOCKER_IMAGE=ubuntu:bionic + - TEST=clang-tidy ROS_DISTRO=melodic DOCKER_IMAGE=ubuntu:bionic - ROS_DISTRO=hydro DOCKER_IMAGE=ubuntu:precise - OPENCV_VERSION=2 ROS_DISTRO=indigo DOCKER_IMAGE=ubuntu:trusty - OPENCV_VERSION=3 ROS_DISTRO=indigo DOCKER_IMAGE=ubuntu:trusty @@ -12,7 +15,7 @@ script: - export CI_SOURCE_PATH=$(pwd) - export REPOSITORY_NAME=${PWD##*/} - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" - - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "OPENCV_VERSION=$OPENCV_VERSION" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" + - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "OPENCV_VERSION=$OPENCV_VERSION" -e "TEST=$TEST" -t $DOCKER_IMAGE sh -c "cd $CI_SOURCE_PATH; /bin/bash .travis.sh" after_failure: - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \; From 1557e0a0c0032de3c17c0ec4d2029a0ccd5ada60 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sun, 21 Apr 2019 09:04:33 +0000 Subject: [PATCH 2/6] fix CMakeLists.txt and package.xml for catkin_lint --- CMakeLists.txt | 7 ++++--- cfg/HSIColorFilter.cfg | 18 ------------------ package.xml | 3 +++ 3 files changed, 7 insertions(+), 21 deletions(-) delete mode 100755 cfg/HSIColorFilter.cfg diff --git a/CMakeLists.txt b/CMakeLists.txt index 9de50ae1..c5eebb87 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(opencv_apps) -find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport nodelet roscpp sensor_msgs) +find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport nodelet roscpp sensor_msgs std_msgs std_srvs) find_package(OpenCV REQUIRED) message(STATUS "OpenCV VERSION: ${OpenCV_VERSION}") @@ -99,7 +99,7 @@ generate_messages( std_msgs ) -catkin_package(CATKIN_DEPENDS std_msgs +catkin_package(CATKIN_DEPENDS message_runtime sensor_msgs std_msgs # DEPENDS OpenCV INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} @@ -351,7 +351,8 @@ install(DIRECTORY launch test scripts ## test if(CATKIN_ENABLE_TESTING) - find_package(catkin REQUIRED COMPONENTS rostest roslaunch) + find_package(rostest REQUIRED) + find_package(roslaunch REQUIRED) if(roslaunch_VERSION VERSION_LESS "1.11.1") message(WARNING "roslaunch_add_file check fails with unsupported doc attributes ${roslaunch_VERSION}") else() diff --git a/cfg/HSIColorFilter.cfg b/cfg/HSIColorFilter.cfg deleted file mode 100755 index 3cf30969..00000000 --- a/cfg/HSIColorFilter.cfg +++ /dev/null @@ -1,18 +0,0 @@ -#!/usr/bin/env python - -PACKAGE = "opencv_apps" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", False) - -gen.add ("h_limit_max", int_t, 0, "The maximum allowed field value Hue", 255, 0, 255) -gen.add ("h_limit_min", int_t, 0, "The minimum allowed field value Hue", 0, 0, 255) -gen.add ("s_limit_max", int_t, 0, "The maximum allowed field value Saturation", 255, 0, 255) -gen.add ("s_limit_min", int_t, 0, "The minimum allowed field value Saturation", 0, 0, 255) -gen.add ("i_limit_max", int_t, 0, "The maximum allowed field value Intensity", 255, 0, 255) -gen.add ("i_limit_min", int_t, 0, "The minimum allowed field value Intensity", 0, 0, 255) - -exit(gen.generate(PACKAGE, "color_filter", "HSIColorFilter")) diff --git a/package.xml b/package.xml index cbed5958..eeba497d 100644 --- a/package.xml +++ b/package.xml @@ -24,6 +24,7 @@ message_generation nodelet roscpp + sensor_msgs std_msgs std_srvs @@ -33,9 +34,11 @@ message_runtime nodelet roscpp + sensor_msgs std_msgs std_srvs + roslaunch rostest rosbag rosservice From a8ce75e897b06373f2105867165ee0ee5fa626df Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sun, 21 Apr 2019 09:45:02 +0000 Subject: [PATCH 3/6] fix format by clang-format --- include/opencv_apps/nodelet.h | 492 +++++---- src/nodelet/adding_images_nodelet.cpp | 369 +++---- src/nodelet/camshift_nodelet.cpp | 192 ++-- src/nodelet/color_filter_nodelet.cpp | 218 ++-- src/nodelet/contour_moments_nodelet.cpp | 105 +- src/nodelet/convex_hull_nodelet.cpp | 87 +- src/nodelet/corner_harris_nodelet.cpp | 78 +- .../discrete_fourier_transform_nodelet.cpp | 185 ++-- src/nodelet/edge_detection_nodelet.cpp | 224 +++-- src/nodelet/face_detection_nodelet.cpp | 112 ++- src/nodelet/face_recognition_nodelet.cpp | 936 ++++++++++-------- src/nodelet/fback_flow_nodelet.cpp | 99 +- src/nodelet/find_contours_nodelet.cpp | 92 +- src/nodelet/general_contours_nodelet.cpp | 97 +- src/nodelet/goodfeature_track_nodelet.cpp | 87 +- src/nodelet/hough_circles_nodelet.cpp | 144 +-- src/nodelet/hough_lines_nodelet.cpp | 176 ++-- src/nodelet/lk_flow_nodelet.cpp | 127 +-- src/nodelet/nodelet.cpp | 244 +++-- src/nodelet/people_detect_nodelet.cpp | 77 +- src/nodelet/phase_corr_nodelet.cpp | 73 +- src/nodelet/pyramids_nodelet.cpp | 67 +- src/nodelet/segment_objects_nodelet.cpp | 100 +- .../simple_compressed_example_nodelet.cpp | 201 ++-- src/nodelet/simple_example_nodelet.cpp | 56 +- src/nodelet/simple_flow_nodelet.cpp | 108 +- src/nodelet/smoothing_nodelet.cpp | 102 +- src/nodelet/threshold_nodelet.cpp | 227 ++--- .../watershed_segmentation_nodelet.cpp | 149 +-- 29 files changed, 2826 insertions(+), 2398 deletions(-) diff --git a/include/opencv_apps/nodelet.h b/include/opencv_apps/nodelet.h index 5418806f..f731eb14 100644 --- a/include/opencv_apps/nodelet.h +++ b/include/opencv_apps/nodelet.h @@ -42,277 +42,243 @@ namespace opencv_apps { +/** @brief + * Enum to represent connection status. + */ +enum ConnectionStatus +{ + NOT_INITIALIZED, + NOT_SUBSCRIBED, + SUBSCRIBED +}; + +/** @brief + * Nodelet to automatically subscribe/unsubscribe + * topics according to subscription of advertised topics. + * + * It's important not to subscribe topic if no output is required. + * + * In order to watch advertised topics, need to use advertise template method. + * And create subscribers in subscribe() and shutdown them in unsubscribed(). + * + */ +class Nodelet : public nodelet::Nodelet +{ +public: + Nodelet() : subscribed_(false) + { + } + +protected: /** @brief - * Enum to represent connection status. + * Initialize nodehandles nh_ and pnh_. Subclass should call + * this method in its onInit method */ - enum ConnectionStatus - { - NOT_INITIALIZED, - NOT_SUBSCRIBED, - SUBSCRIBED - }; - + virtual void onInit(); + + /** @brief + * Post processing of initialization of nodelet. + * You need to call this method in order to use always_subscribe + * feature. + */ + virtual void onInitPostProcess(); + + /** @brief + * callback function which is called when new subscriber come + */ + virtual void connectionCallback(const ros::SingleSubscriberPublisher& pub); + + /** @brief + * callback function which is called when new subscriber come for image + * publisher + */ + virtual void imageConnectionCallback(const image_transport::SingleSubscriberPublisher& pub); + + /** @brief + * callback function which is called when new subscriber come for camera + * image publisher + */ + virtual void cameraConnectionCallback(const image_transport::SingleSubscriberPublisher& pub); + + /** @brief + * callback function which is called when new subscriber come for + * camera info publisher + */ + virtual void cameraInfoConnectionCallback(const ros::SingleSubscriberPublisher& pub); + + /** @brief + * callback function which is called when new subscriber come for camera + * image publisher or camera info publisher. + * This function is called from cameraConnectionCallback + * or cameraInfoConnectionCallback. + */ + virtual void cameraConnectionBaseCallback(); + + /** @brief + * callback function which is called when walltimer + * duration run out. + */ + virtual void warnNeverSubscribedCallback(const ros::WallTimerEvent& event); + + /** @brief + * This method is called when publisher is subscribed by other + * nodes. + * Set up subscribers in this method. + */ + virtual void subscribe() = 0; + /** @brief - * Nodelet to automatically subscribe/unsubscribe - * topics according to subscription of advertised topics. + * This method is called when publisher is unsubscribed by other + * nodes. + * Shut down subscribers in this method. + */ + virtual void unsubscribe() = 0; + + /** @brief + * Advertise a topic and watch the publisher. Publishers which are + * created by this method. + * It automatically reads latch boolean parameter from nh and + * publish topic with appropriate latch parameter. * - * It's important not to subscribe topic if no output is required. + * @param nh NodeHandle. + * @param topic topic name to advertise. + * @param queue_size queue size for publisher. + * @param latch set true if latch topic publication. + * @return Publisher for the advertised topic. + */ + template + ros::Publisher advertise(ros::NodeHandle& nh, std::string topic, int queue_size) + { + boost::mutex::scoped_lock lock(connection_mutex_); + ros::SubscriberStatusCallback connect_cb = boost::bind(&Nodelet::connectionCallback, this, _1); + ros::SubscriberStatusCallback disconnect_cb = boost::bind(&Nodelet::connectionCallback, this, _1); + bool latch; + nh.param("latch", latch, false); + ros::Publisher ret = nh.advertise(topic, queue_size, connect_cb, disconnect_cb, ros::VoidConstPtr(), latch); + publishers_.push_back(ret); + + return ret; + } + + /** @brief + * Advertise an image topic and watch the publisher. Publishers which are + * created by this method. + * It automatically reads latch boolean parameter from nh and it and + * publish topic with appropriate latch parameter. * - * In order to watch advertised topics, need to use advertise template method. - * And create subscribers in subscribe() and shutdown them in unsubscribed(). + * @param nh NodeHandle. + * @param topic topic name to advertise. + * @param queue_size queue size for publisher. + * @param latch set true if latch topic publication. + * @return Publisher for the advertised topic. + */ + image_transport::Publisher advertiseImage(ros::NodeHandle& nh, const std::string& topic, int queue_size) + { + boost::mutex::scoped_lock lock(connection_mutex_); + image_transport::SubscriberStatusCallback connect_cb = boost::bind(&Nodelet::imageConnectionCallback, this, _1); + image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&Nodelet::imageConnectionCallback, this, _1); + bool latch; + nh.param("latch", latch, false); + image_transport::Publisher pub = + image_transport::ImageTransport(nh).advertise(topic, 1, connect_cb, disconnect_cb, ros::VoidPtr(), latch); + image_publishers_.push_back(pub); + return pub; + } + + /** @brief + * Advertise an image topic camera info topic and watch the publisher. + * Publishers which are + * created by this method. + * It automatically reads latch boolean parameter from nh and it and + * publish topic with appropriate latch parameter. * + * @param nh NodeHandle. + * @param topic topic name to advertise. + * @param queue_size queue size for publisher. + * @param latch set true if latch topic publication. + * @return Publisher for the advertised topic. */ - class Nodelet: public nodelet::Nodelet + image_transport::CameraPublisher advertiseCamera(ros::NodeHandle& nh, const std::string& topic, int queue_size) { - public: - Nodelet(): subscribed_(false) { } - protected: - - /** @brief - * Initialize nodehandles nh_ and pnh_. Subclass should call - * this method in its onInit method - */ - virtual void onInit(); - - - /** @brief - * Post processing of initialization of nodelet. - * You need to call this method in order to use always_subscribe - * feature. - */ - virtual void onInitPostProcess(); - - /** @brief - * callback function which is called when new subscriber come - */ - virtual void connectionCallback(const ros::SingleSubscriberPublisher& pub); - - /** @brief - * callback function which is called when new subscriber come for image - * publisher - */ - virtual void imageConnectionCallback( - const image_transport::SingleSubscriberPublisher& pub); - - /** @brief - * callback function which is called when new subscriber come for camera - * image publisher - */ - virtual void cameraConnectionCallback( - const image_transport::SingleSubscriberPublisher& pub); - - /** @brief - * callback function which is called when new subscriber come for - * camera info publisher - */ - virtual void cameraInfoConnectionCallback( - const ros::SingleSubscriberPublisher& pub); - - /** @brief - * callback function which is called when new subscriber come for camera - * image publisher or camera info publisher. - * This function is called from cameraConnectionCallback - * or cameraInfoConnectionCallback. - */ - virtual void cameraConnectionBaseCallback(); - - /** @brief - * callback function which is called when walltimer - * duration run out. - */ - virtual void warnNeverSubscribedCallback(const ros::WallTimerEvent& event); - - /** @brief - * This method is called when publisher is subscribed by other - * nodes. - * Set up subscribers in this method. - */ - virtual void subscribe() = 0; - - /** @brief - * This method is called when publisher is unsubscribed by other - * nodes. - * Shut down subscribers in this method. - */ - virtual void unsubscribe() = 0; - - /** @brief - * Advertise a topic and watch the publisher. Publishers which are - * created by this method. - * It automatically reads latch boolean parameter from nh and - * publish topic with appropriate latch parameter. - * - * @param nh NodeHandle. - * @param topic topic name to advertise. - * @param queue_size queue size for publisher. - * @param latch set true if latch topic publication. - * @return Publisher for the advertised topic. - */ - template ros::Publisher - advertise(ros::NodeHandle& nh, - std::string topic, int queue_size) - { - boost::mutex::scoped_lock lock(connection_mutex_); - ros::SubscriberStatusCallback connect_cb - = boost::bind(&Nodelet::connectionCallback, this, _1); - ros::SubscriberStatusCallback disconnect_cb - = boost::bind(&Nodelet::connectionCallback, this, _1); - bool latch; - nh.param("latch", latch, false); - ros::Publisher ret = nh.advertise(topic, queue_size, - connect_cb, - disconnect_cb, - ros::VoidConstPtr(), - latch); - publishers_.push_back(ret); - - return ret; - } - - /** @brief - * Advertise an image topic and watch the publisher. Publishers which are - * created by this method. - * It automatically reads latch boolean parameter from nh and it and - * publish topic with appropriate latch parameter. - * - * @param nh NodeHandle. - * @param topic topic name to advertise. - * @param queue_size queue size for publisher. - * @param latch set true if latch topic publication. - * @return Publisher for the advertised topic. - */ - image_transport::Publisher - advertiseImage(ros::NodeHandle& nh, - const std::string& topic, - int queue_size) - { - boost::mutex::scoped_lock lock(connection_mutex_); - image_transport::SubscriberStatusCallback connect_cb - = boost::bind(&Nodelet::imageConnectionCallback, - this, _1); - image_transport::SubscriberStatusCallback disconnect_cb - = boost::bind(&Nodelet::imageConnectionCallback, - this, _1); - bool latch; - nh.param("latch", latch, false); - image_transport::Publisher pub - = image_transport::ImageTransport(nh).advertise( - topic, 1, - connect_cb, disconnect_cb, - ros::VoidPtr(), latch); - image_publishers_.push_back(pub); - return pub; - } - - /** @brief - * Advertise an image topic camera info topic and watch the publisher. - * Publishers which are - * created by this method. - * It automatically reads latch boolean parameter from nh and it and - * publish topic with appropriate latch parameter. - * - * @param nh NodeHandle. - * @param topic topic name to advertise. - * @param queue_size queue size for publisher. - * @param latch set true if latch topic publication. - * @return Publisher for the advertised topic. - */ - image_transport::CameraPublisher - advertiseCamera(ros::NodeHandle& nh, - const std::string& topic, - int queue_size) - { - boost::mutex::scoped_lock lock(connection_mutex_); - image_transport::SubscriberStatusCallback connect_cb - = boost::bind(&Nodelet::cameraConnectionCallback, - this, _1); - image_transport::SubscriberStatusCallback disconnect_cb - = boost::bind(&Nodelet::cameraConnectionCallback, - this, _1); - ros::SubscriberStatusCallback info_connect_cb - = boost::bind(&Nodelet::cameraInfoConnectionCallback, - this, _1); - ros::SubscriberStatusCallback info_disconnect_cb - = boost::bind(&Nodelet::cameraInfoConnectionCallback, - this, _1); - bool latch; - nh.param("latch", latch, false); - image_transport::CameraPublisher - pub = image_transport::ImageTransport(nh).advertiseCamera(topic, 1, - connect_cb, disconnect_cb, - info_connect_cb, info_disconnect_cb, - ros::VoidPtr(), - latch); - camera_publishers_.push_back(pub); - return pub; - } - - /** @brief - * mutex to call subscribe() and unsubscribe() in - * critical section. - */ - boost::mutex connection_mutex_; - - /** @brief - * List of watching publishers - */ - std::vector publishers_; - - /** @brief - * List of watching image publishers - */ - std::vector image_publishers_; - - /** @brief - * List of watching camera publishers - */ - std::vector camera_publishers_; - - /** @brief - * Shared pointer to nodehandle. - */ - boost::shared_ptr nh_; - - /** @brief - * Shared pointer to private nodehandle. - */ - boost::shared_ptr pnh_; - - /** @brief - * WallTimer instance for warning about no connection. - */ - ros::WallTimer timer_; - - /** @brief - * A flag to check if any publisher is already subscribed - * or not. - */ - bool subscribed_; - - /** @brief - * A flag to check if the node has been ever subscribed - * or not. - */ - bool ever_subscribed_; - - /** @brief - * A flag to disable watching mechanism and always subscribe input - * topics. It can be specified via ~always_subscribe parameter. - */ - bool always_subscribe_; - - /** @brief - * Status of connection - */ - ConnectionStatus connection_status_; - - /** @brief - * true if `~verbose_connection` or `verbose_connection` parameter is true. - */ - bool verbose_connection_; - private: - - }; + boost::mutex::scoped_lock lock(connection_mutex_); + image_transport::SubscriberStatusCallback connect_cb = boost::bind(&Nodelet::cameraConnectionCallback, this, _1); + image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&Nodelet::cameraConnectionCallback, this, _1); + ros::SubscriberStatusCallback info_connect_cb = boost::bind(&Nodelet::cameraInfoConnectionCallback, this, _1); + ros::SubscriberStatusCallback info_disconnect_cb = boost::bind(&Nodelet::cameraInfoConnectionCallback, this, _1); + bool latch; + nh.param("latch", latch, false); + image_transport::CameraPublisher pub = image_transport::ImageTransport(nh).advertiseCamera( + topic, 1, connect_cb, disconnect_cb, info_connect_cb, info_disconnect_cb, ros::VoidPtr(), latch); + camera_publishers_.push_back(pub); + return pub; + } + + /** @brief + * mutex to call subscribe() and unsubscribe() in + * critical section. + */ + boost::mutex connection_mutex_; + + /** @brief + * List of watching publishers + */ + std::vector publishers_; + + /** @brief + * List of watching image publishers + */ + std::vector image_publishers_; + + /** @brief + * List of watching camera publishers + */ + std::vector camera_publishers_; + + /** @brief + * Shared pointer to nodehandle. + */ + boost::shared_ptr nh_; + + /** @brief + * Shared pointer to private nodehandle. + */ + boost::shared_ptr pnh_; + + /** @brief + * WallTimer instance for warning about no connection. + */ + ros::WallTimer timer_; + + /** @brief + * A flag to check if any publisher is already subscribed + * or not. + */ + bool subscribed_; + + /** @brief + * A flag to check if the node has been ever subscribed + * or not. + */ + bool ever_subscribed_; + + /** @brief + * A flag to disable watching mechanism and always subscribe input + * topics. It can be specified via ~always_subscribe parameter. + */ + bool always_subscribe_; + + /** @brief + * Status of connection + */ + ConnectionStatus connection_status_; + + /** @brief + * true if `~verbose_connection` or `verbose_connection` parameter is true. + */ + bool verbose_connection_; + +private: +}; } #endif diff --git a/src/nodelet/adding_images_nodelet.cpp b/src/nodelet/adding_images_nodelet.cpp index 514f8b63..f4d5cd0e 100644 --- a/src/nodelet/adding_images_nodelet.cpp +++ b/src/nodelet/adding_images_nodelet.cpp @@ -55,221 +55,234 @@ #include "opencv_apps/AddingImagesConfig.h" #include "opencv_apps/nodelet.h" -namespace opencv_apps { - class AddingImagesNodelet : public opencv_apps::Nodelet { - private: - boost::shared_ptr it_; +namespace opencv_apps +{ +class AddingImagesNodelet : public opencv_apps::Nodelet +{ +private: + boost::shared_ptr it_; - typedef message_filters::sync_policies::ExactTime< - sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicyWithCameraInfo; - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> ApproxSyncPolicyWithCameraInfo; - typedef message_filters::sync_policies::ExactTime< - sensor_msgs::Image, sensor_msgs::Image> SyncPolicy; - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::Image, sensor_msgs::Image> ApproxSyncPolicy; + typedef message_filters::sync_policies::ExactTime + SyncPolicyWithCameraInfo; + typedef message_filters::sync_policies::ApproximateTime + ApproxSyncPolicyWithCameraInfo; + typedef message_filters::sync_policies::ExactTime SyncPolicy; + typedef message_filters::sync_policies::ApproximateTime ApproxSyncPolicy; - //////////////////////////////////////////////////////// - // Dynamic Reconfigure - //////////////////////////////////////////////////////// - typedef opencv_apps::AddingImagesConfig Config; - typedef dynamic_reconfigure::Server ReconfigureServer; - Config config_; - boost::shared_ptr reconfigure_server_; + //////////////////////////////////////////////////////// + // Dynamic Reconfigure + //////////////////////////////////////////////////////// + typedef opencv_apps::AddingImagesConfig Config; + typedef dynamic_reconfigure::Server ReconfigureServer; + Config config_; + boost::shared_ptr reconfigure_server_; - int queue_size_; - bool debug_view_; - ros::Time prev_stamp_; + int queue_size_; + bool debug_view_; + ros::Time prev_stamp_; - std::string window_name_; + std::string window_name_; - image_transport::SubscriberFilter sub_image1_, sub_image2_; - image_transport::Publisher img_pub_; - message_filters::Subscriber sub_camera_info_; - boost::shared_ptr > sync_with_info_; - boost::shared_ptr > async_with_info_; - boost::shared_ptr > sync_; - boost::shared_ptr > async_; - boost::mutex mutex_; + image_transport::SubscriberFilter sub_image1_, sub_image2_; + image_transport::Publisher img_pub_; + message_filters::Subscriber sub_camera_info_; + boost::shared_ptr > sync_with_info_; + boost::shared_ptr > async_with_info_; + boost::shared_ptr > sync_; + boost::shared_ptr > async_; + boost::mutex mutex_; - bool approximate_sync_; - double alpha_; - double beta_; - double gamma_; + bool approximate_sync_; + double alpha_; + double beta_; + double gamma_; - void imageCallbackWithInfo( - const sensor_msgs::ImageConstPtr& msg1, - const sensor_msgs::ImageConstPtr& msg2, - const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg1, msg2, cam_info->header.frame_id); - } + void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg1, const sensor_msgs::ImageConstPtr& msg2, + const sensor_msgs::CameraInfoConstPtr& cam_info) + { + do_work(msg1, msg2, cam_info->header.frame_id); + } - void imageCallback(const sensor_msgs::ImageConstPtr& msg1, - const sensor_msgs::ImageConstPtr& msg2) { - do_work(msg1, msg2, msg1->header.frame_id); - } + void imageCallback(const sensor_msgs::ImageConstPtr& msg1, const sensor_msgs::ImageConstPtr& msg2) + { + do_work(msg1, msg2, msg1->header.frame_id); + } - void subscribe() { - NODELET_DEBUG("Subscribing to image topic."); - sub_image1_.subscribe(*it_, "image1", 3); - sub_image2_.subscribe(*it_, "image2", 3); - sub_camera_info_.subscribe(*nh_, "camera_info", 3); - if (config_.use_camera_info) { - if (approximate_sync_) { - async_with_info_ = boost::make_shared< - message_filters::Synchronizer >(queue_size_); - async_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_); - async_with_info_->registerCallback(boost::bind( - &AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3)); - } else { - sync_with_info_ = - boost::make_shared >(queue_size_); - sync_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_); - sync_with_info_->registerCallback(boost::bind( - &AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3)); - } - } else { - if (approximate_sync_) { - async_ = boost::make_shared< - message_filters::Synchronizer >(queue_size_); - async_->connectInput(sub_image1_, sub_image2_); - async_->registerCallback( - boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2)); - } else { - sync_ = - boost::make_shared >(queue_size_); - sync_->connectInput(sub_image1_, sub_image2_); - sync_->registerCallback( - boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2)); - } + void subscribe() + { + NODELET_DEBUG("Subscribing to image topic."); + sub_image1_.subscribe(*it_, "image1", 3); + sub_image2_.subscribe(*it_, "image2", 3); + sub_camera_info_.subscribe(*nh_, "camera_info", 3); + if (config_.use_camera_info) + { + if (approximate_sync_) + { + async_with_info_ = + boost::make_shared >(queue_size_); + async_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_); + async_with_info_->registerCallback(boost::bind(&AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3)); + } + else + { + sync_with_info_ = boost::make_shared >(queue_size_); + sync_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_); + sync_with_info_->registerCallback(boost::bind(&AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3)); } } - - void unsubscribe() { - NODELET_DEBUG("Unsubscribing from image topic."); - sub_image1_.unsubscribe(); - sub_image2_.unsubscribe(); - sub_camera_info_.unsubscribe(); + else + { + if (approximate_sync_) + { + async_ = boost::make_shared >(queue_size_); + async_->connectInput(sub_image1_, sub_image2_); + async_->registerCallback(boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2)); + } + else + { + sync_ = boost::make_shared >(queue_size_); + sync_->connectInput(sub_image1_, sub_image2_); + sync_->registerCallback(boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2)); + } } + } - void reconfigureCallback(Config& config, uint32_t level) { - boost::mutex::scoped_lock lock(mutex_); - config_ = config; - alpha_ = config.alpha; - if ( config.auto_beta ) { - beta_ = 1.0 - alpha_; - config.beta = beta_; - } else { - beta_ = config.beta; - } - gamma_ = config.gamma; + void unsubscribe() + { + NODELET_DEBUG("Unsubscribing from image topic."); + sub_image1_.unsubscribe(); + sub_image2_.unsubscribe(); + sub_camera_info_.unsubscribe(); + } + + void reconfigureCallback(Config& config, uint32_t level) + { + boost::mutex::scoped_lock lock(mutex_); + config_ = config; + alpha_ = config.alpha; + if (config.auto_beta) + { + beta_ = 1.0 - alpha_; + config.beta = beta_; + } + else + { + beta_ = config.beta; } + gamma_ = config.gamma; + } - void do_work(const sensor_msgs::Image::ConstPtr& image_msg1, - const sensor_msgs::Image::ConstPtr& image_msg2, - const std::string input_frame_from_msg) { - boost::mutex::scoped_lock lock(mutex_); - // Work on the image. - try { - cv::Mat image1 = - cv_bridge::toCvCopy(image_msg1, image_msg1->encoding)->image; - cv::Mat image2 = - cv_bridge::toCvCopy(image_msg2, image_msg1->encoding)->image; - if (cv_bridge::getCvType(image_msg1->encoding) != cv_bridge::getCvType(image_msg2->encoding)) { - NODELET_ERROR("Encoding of input images must be same type: %s, %s", - image_msg1->encoding.c_str(), image_msg2->encoding.c_str()); - return; - } + void do_work(const sensor_msgs::Image::ConstPtr& image_msg1, const sensor_msgs::Image::ConstPtr& image_msg2, + const std::string input_frame_from_msg) + { + boost::mutex::scoped_lock lock(mutex_); + // Work on the image. + try + { + cv::Mat image1 = cv_bridge::toCvCopy(image_msg1, image_msg1->encoding)->image; + cv::Mat image2 = cv_bridge::toCvCopy(image_msg2, image_msg1->encoding)->image; + if (cv_bridge::getCvType(image_msg1->encoding) != cv_bridge::getCvType(image_msg2->encoding)) + { + NODELET_ERROR("Encoding of input images must be same type: %s, %s", image_msg1->encoding.c_str(), + image_msg2->encoding.c_str()); + return; + } - cv::Mat result_image; - if ( image1.rows != image2.rows || image1.cols != image2.cols ) { - int new_rows = std::max(image1.rows, image2.rows); - int new_cols = std::max(image1.cols, image2.cols); - // if ( new_rows != image1.rows || new_cols != image1.cols ) { - cv::Mat image1_ = cv::Mat(new_rows, new_cols, image1.type()); - image1.copyTo(image1_(cv::Rect(0, 0, image1.cols, image1.rows))); - image1 = image1_.clone(); // need clone becuase toCvShare?? + cv::Mat result_image; + if (image1.rows != image2.rows || image1.cols != image2.cols) + { + int new_rows = std::max(image1.rows, image2.rows); + int new_cols = std::max(image1.cols, image2.cols); + // if ( new_rows != image1.rows || new_cols != image1.cols ) { + cv::Mat image1_ = cv::Mat(new_rows, new_cols, image1.type()); + image1.copyTo(image1_(cv::Rect(0, 0, image1.cols, image1.rows))); + image1 = image1_.clone(); // need clone becuase toCvShare?? - //if ( new_rows != image2.rows || new_cols != image2.cols ) { - cv::Mat image2_ = cv::Mat(new_rows, new_cols, image2.type()); - image2.copyTo(image2_(cv::Rect(0, 0, image2.cols, image2.rows))); - image2 = image2_.clone(); - } - cv::addWeighted(image1, alpha_, image2, beta_, gamma_, result_image); - //-- Show what you got - sensor_msgs::ImagePtr image_msg3 = cv_bridge::CvImage(image_msg1->header, - image_msg1->encoding, - result_image).toImageMsg(); - if (debug_view_) { - cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + // if ( new_rows != image2.rows || new_cols != image2.cols ) { + cv::Mat image2_ = cv::Mat(new_rows, new_cols, image2.type()); + image2.copyTo(image2_(cv::Rect(0, 0, image2.cols, image2.rows))); + image2 = image2_.clone(); + } + cv::addWeighted(image1, alpha_, image2, beta_, gamma_, result_image); + //-- Show what you got + sensor_msgs::ImagePtr image_msg3 = + cv_bridge::CvImage(image_msg1->header, image_msg1->encoding, result_image).toImageMsg(); + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); #ifdef CV_BRIDGE_CVT_COLOR_FOR_DISPLAY_IS_NOT_SUPPORTED - cv::imshow(window_name_, result_image); + cv::imshow(window_name_, result_image); #else #ifdef CV_BRIDGE_CVT_COLOR_FOR_DISPLAY_OPTION_IS_NOT_SUPPORTED - cv::imshow(window_name_, cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg3, image_msg3->encoding))->image); + cv::imshow(window_name_, + cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg3, image_msg3->encoding))->image); #else - cv_bridge::CvtColorForDisplayOptions options; - if (sensor_msgs::image_encodings::bitDepth(image_msg1->encoding) == 32 || - sensor_msgs::image_encodings::bitDepth(image_msg1->encoding) == 64) { - // float or double image - options.do_dynamic_scaling = true; - } - cv::imshow(window_name_, cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg3), "", options)->image); + cv_bridge::CvtColorForDisplayOptions options; + if (sensor_msgs::image_encodings::bitDepth(image_msg1->encoding) == 32 || + sensor_msgs::image_encodings::bitDepth(image_msg1->encoding) == 64) + { + // float or double image + options.do_dynamic_scaling = true; + } + cv::imshow(window_name_, cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg3), "", options)->image); #endif #endif - int c = cv::waitKey(1); - } - img_pub_.publish(image_msg3); - - } catch (cv::Exception& e) { - NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), - e.func.c_str(), e.file.c_str(), e.line); + int c = cv::waitKey(1); } - - prev_stamp_ = image_msg1->header.stamp; + img_pub_.publish(image_msg3); + } + catch (cv::Exception& e) + { + NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } - public: - virtual void onInit() { - Nodelet::onInit(); - it_ = boost::shared_ptr( - new image_transport::ImageTransport(*nh_)); + prev_stamp_ = image_msg1->header.stamp; + } - pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { - always_subscribe_ = true; - } - prev_stamp_ = ros::Time(0, 0); +public: + virtual void onInit() + { + Nodelet::onInit(); + it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + + pnh_->param("debug_view", debug_view_, false); + if (debug_view_) + { + always_subscribe_ = true; + } + prev_stamp_ = ros::Time(0, 0); - window_name_ = "AddingImages Demo"; - //////////////////////////////////////////////////////// - // Dynamic Reconfigure - //////////////////////////////////////////////////////// - reconfigure_server_ = - boost::make_shared >(*pnh_); - dynamic_reconfigure::Server::CallbackType f = + window_name_ = "AddingImages Demo"; + //////////////////////////////////////////////////////// + // Dynamic Reconfigure + //////////////////////////////////////////////////////// + reconfigure_server_ = boost::make_shared >(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind(&AddingImagesNodelet::reconfigureCallback, this, _1, _2); - reconfigure_server_->setCallback(f); + reconfigure_server_->setCallback(f); - pnh_->param("approximate_sync", approximate_sync_, true); - pnh_->param("queue_size", queue_size_, 100); - img_pub_ = advertiseImage(*pnh_, "image", 1); - onInitPostProcess(); - } - }; -} // namespace opencv_apps + pnh_->param("approximate_sync", approximate_sync_, true); + pnh_->param("queue_size", queue_size_, 100); + img_pub_ = advertiseImage(*pnh_, "image", 1); + onInitPostProcess(); + } +}; +} // namespace opencv_apps -namespace adding_images { -class AddingImagesNodelet : public opencv_apps::AddingImagesNodelet { +namespace adding_images +{ +class AddingImagesNodelet : public opencv_apps::AddingImagesNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet adding_images/adding_images is deprecated, " "and renamed to opencv_apps/adding_images."); opencv_apps::AddingImagesNodelet::onInit(); } }; -} // namespace adding_images - +} // namespace adding_images #include PLUGINLIB_EXPORT_CLASS(opencv_apps::AddingImagesNodelet, nodelet::Nodelet); diff --git a/src/nodelet/camshift_nodelet.cpp b/src/nodelet/camshift_nodelet.cpp index 494ba306..512fef50 100644 --- a/src/nodelet/camshift_nodelet.cpp +++ b/src/nodelet/camshift_nodelet.cpp @@ -3,11 +3,11 @@ * * Copyright (c) 2014, Kei Okada. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -56,7 +56,8 @@ #include "opencv_apps/CamShiftConfig.h" #include "opencv_apps/RotatedRectStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class CamShiftNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_, bproj_pub_; @@ -96,9 +97,9 @@ class CamShiftNodelet : public opencv_apps::Nodelet float hranges[2]; const float* phranges; cv::Mat hist, histimg; - //cv::Mat hsv; - - static void onMouse( int event, int x, int y, int, void* ) + // cv::Mat hsv; + + static void onMouse(int event, int x, int y, int, void*) { on_mouse_update_ = true; on_mouse_event_ = event; @@ -106,7 +107,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet on_mouse_y_ = y; } - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; vmin_ = config_.vmin; @@ -114,7 +115,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet smin_ = config_.smin; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -125,13 +126,13 @@ class CamShiftNodelet : public opencv_apps::Nodelet { do_work(msg, cam_info->header.frame_id); } - + void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -144,24 +145,26 @@ class CamShiftNodelet : public opencv_apps::Nodelet // Convert the image into something opencv can handle. cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; cv::Mat backproj; - + // Messages opencv_apps::RotatedRectStamped rect_msg; rect_msg.header = msg->header; // Do the work - - if( debug_view_) { + + if (debug_view_) + { /// Create Trackbars for Thresholds - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); - cv::setMouseCallback( window_name_, onMouse, 0 ); - cv::createTrackbar( "Vmin", window_name_, &vmin_, 256, trackbarCallback); - cv::createTrackbar( "Vmax", window_name_, &vmax_, 256, trackbarCallback); - cv::createTrackbar( "Smin", window_name_, &smin_, 256, trackbarCallback); + cv::setMouseCallback(window_name_, onMouse, 0); + cv::createTrackbar("Vmin", window_name_, &vmin_, 256, trackbarCallback); + cv::createTrackbar("Vmax", window_name_, &vmax_, 256, trackbarCallback); + cv::createTrackbar("Smin", window_name_, &smin_, 256, trackbarCallback); - if (need_config_update_) { + if (need_config_update_) + { config_.vmin = vmin_; config_.vmax = vmax_; config_.smin = smin_; @@ -170,12 +173,13 @@ class CamShiftNodelet : public opencv_apps::Nodelet } } - if ( on_mouse_update_ ) { + if (on_mouse_update_) + { int event = on_mouse_event_; int x = on_mouse_x_; int y = on_mouse_y_; - if( selectObject ) + if (selectObject) { selection.x = MIN(x, origin.x); selection.y = MIN(y, origin.y); @@ -185,45 +189,47 @@ class CamShiftNodelet : public opencv_apps::Nodelet selection &= cv::Rect(0, 0, frame.cols, frame.rows); } - switch( event ) + switch (event) { case cv::EVENT_LBUTTONDOWN: - origin = cv::Point(x,y); - selection = cv::Rect(x,y,0,0); + origin = cv::Point(x, y); + selection = cv::Rect(x, y, 0, 0); selectObject = true; break; case cv::EVENT_LBUTTONUP: selectObject = false; - if( selection.width > 0 && selection.height > 0 ) + if (selection.width > 0 && selection.height > 0) trackObject = -1; break; } on_mouse_update_ = false; } - if( !paused ) + if (!paused) { cv::Mat hsv, hue, mask; cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV); - if( trackObject ) + if (trackObject) { int _vmin = vmin_, _vmax = vmax_; - cv::inRange(hsv, cv::Scalar(0, smin_, MIN(_vmin,_vmax)), - cv::Scalar(180, 256, MAX(_vmin, _vmax)), mask); - int ch[] = {0, 0}; + cv::inRange(hsv, cv::Scalar(0, smin_, MIN(_vmin, _vmax)), cv::Scalar(180, 256, MAX(_vmin, _vmax)), mask); + int ch[] = { 0, 0 }; hue.create(hsv.size(), hsv.depth()); cv::mixChannels(&hsv, 1, &hue, 1, ch, 1); - if( trackObject < 0 ) + if (trackObject < 0) { cv::Mat roi(hue, selection), maskroi(mask, selection); cv::calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges); cv::normalize(hist, hist, 0, 255, cv::NORM_MINMAX); std::vector hist_value; hist_value.resize(hsize); - for(int i = 0; i < hsize; i ++) { hist_value[i] = hist.at(i);} + for (int i = 0; i < hsize; i++) + { + hist_value[i] = hist.at(i); + } pnh_->setParam("histogram", hist_value); trackWindow = selection; @@ -232,41 +238,38 @@ class CamShiftNodelet : public opencv_apps::Nodelet histimg = cv::Scalar::all(0); int binW = histimg.cols / hsize; cv::Mat buf(1, hsize, CV_8UC3); - for( int i = 0; i < hsize; i++ ) - buf.at(i) = cv::Vec3b(cv::saturate_cast(i*180./hsize), 255, 255); + for (int i = 0; i < hsize; i++) + buf.at(i) = cv::Vec3b(cv::saturate_cast(i * 180. / hsize), 255, 255); cv::cvtColor(buf, buf, cv::COLOR_HSV2BGR); - for( int i = 0; i < hsize; i++ ) + for (int i = 0; i < hsize; i++) { - int val = cv::saturate_cast(hist.at(i)*histimg.rows/255); - cv::rectangle( histimg, cv::Point(i*binW,histimg.rows), - cv::Point((i+1)*binW,histimg.rows - val), - cv::Scalar(buf.at(i)), -1, 8 ); + int val = cv::saturate_cast(hist.at(i) * histimg.rows / 255); + cv::rectangle(histimg, cv::Point(i * binW, histimg.rows), cv::Point((i + 1) * binW, histimg.rows - val), + cv::Scalar(buf.at(i)), -1, 8); } } cv::calcBackProject(&hue, 1, 0, hist, backproj, &phranges); backproj &= mask; - cv::RotatedRect trackBox = cv::CamShift(backproj, trackWindow, - cv::TermCriteria( cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 10, 1 )); - if( trackWindow.area() <= 1 ) + cv::RotatedRect trackBox = cv::CamShift( + backproj, trackWindow, cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 10, 1)); + if (trackWindow.area() <= 1) { - int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6; - //trackWindow = cv::Rect(trackWindow.x - r, trackWindow.y - r, + int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5) / 6; + // trackWindow = cv::Rect(trackWindow.x - r, trackWindow.y - r, // trackWindow.x + r, trackWindow.y + r) & - trackWindow = cv::Rect(cols/2 - r, rows/2 - r, - cols/2 + r, rows/2 + r) & - cv::Rect(0, 0, cols, rows); + trackWindow = cv::Rect(cols / 2 - r, rows / 2 - r, cols / 2 + r, rows / 2 + r) & cv::Rect(0, 0, cols, rows); } - if( backprojMode ) - cv::cvtColor( backproj, frame, cv::COLOR_GRAY2BGR ); + if (backprojMode) + cv::cvtColor(backproj, frame, cv::COLOR_GRAY2BGR); #ifndef CV_VERSION_EPOCH - cv::ellipse( frame, trackBox, cv::Scalar(0,0,255), 3, cv::LINE_AA ); + cv::ellipse(frame, trackBox, cv::Scalar(0, 0, 255), 3, cv::LINE_AA); #else - cv::ellipse( frame, trackBox, cv::Scalar(0,0,255), 3, CV_AA ); + cv::ellipse(frame, trackBox, cv::Scalar(0, 0, 255), 3, CV_AA); #endif - + rect_msg.rect.angle = trackBox.angle; opencv_apps::Point2D point_msg; opencv_apps::Size size_msg; @@ -278,23 +281,24 @@ class CamShiftNodelet : public opencv_apps::Nodelet rect_msg.rect.size = size_msg; } } - else if( trackObject < 0 ) + else if (trackObject < 0) paused = false; - if( selectObject && selection.width > 0 && selection.height > 0 ) + if (selectObject && selection.width > 0 && selection.height > 0) { cv::Mat roi(frame, selection); bitwise_not(roi, roi); } - if( debug_view_ ) { - cv::imshow( window_name_, frame ); - cv::imshow( histogram_name_, histimg ); + if (debug_view_) + { + cv::imshow(window_name_, frame); + cv::imshow(histogram_name_, histimg); char c = (char)cv::waitKey(1); - //if( c == 27 ) + // if( c == 27 ) // break; - switch(c) + switch (c) { case 'b': backprojMode = !backprojMode; @@ -305,28 +309,28 @@ class CamShiftNodelet : public opencv_apps::Nodelet break; case 'h': showHist = !showHist; - if( !showHist ) - cv::destroyWindow( histogram_name_ ); + if (!showHist) + cv::destroyWindow(histogram_name_); else - cv::namedWindow( histogram_name_, 1 ); + cv::namedWindow(histogram_name_, 1); break; case 'p': paused = !paused; break; - default: - ; + default:; } } // Publish the image. sensor_msgs::Image::Ptr out_img1 = cv_bridge::CvImage(msg->header, msg->encoding, frame).toImageMsg(); - sensor_msgs::Image::Ptr out_img2 = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, backproj).toImageMsg(); + sensor_msgs::Image::Ptr out_img2 = + cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, backproj).toImageMsg(); img_pub_.publish(out_img1); bproj_pub_.publish(out_img2); - if( trackObject ) + if (trackObject) msg_pub_.publish(rect_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -358,7 +362,8 @@ class CamShiftNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -366,7 +371,9 @@ class CamShiftNodelet : public opencv_apps::Nodelet window_name_ = "CamShift Demo"; histogram_name_ = "Histogram"; - vmin_ = 10; vmax_ = 256; smin_ = 30; + vmin_ = 10; + vmax_ = 256; + smin_ = 30; backprojMode = false; selectObject = false; trackObject = 0; @@ -381,14 +388,13 @@ class CamShiftNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&CamShiftNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&CamShiftNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); - img_pub_ = advertiseImage(*pnh_, "image", 1); bproj_pub_ = advertiseImage(*pnh_, "back_project", 1); msg_pub_ = advertise(*pnh_, "track_box", 1); - + NODELET_INFO("Hot keys: "); NODELET_INFO("\tESC - quit the program"); NODELET_INFO("\tc - stop the tracking"); @@ -399,26 +405,28 @@ class CamShiftNodelet : public opencv_apps::Nodelet std::vector hist_value; pnh_->getParam("histogram", hist_value); - if ( hist_value.size() == hsize ) { + if (hist_value.size() == hsize) + { hist.create(hsize, 1, CV_32F); - for(int i = 0; i < hsize; i ++) { hist.at(i) = hist_value[i];} + for (int i = 0; i < hsize; i++) + { + hist.at(i) = hist_value[i]; + } trackObject = 1; - trackWindow = cv::Rect(0, 0, 640, 480); // - + trackWindow = cv::Rect(0, 0, 640, 480); // histimg = cv::Scalar::all(0); int binW = histimg.cols / hsize; cv::Mat buf(1, hsize, CV_8UC3); - for( int i = 0; i < hsize; i++ ) - buf.at(i) = cv::Vec3b(cv::saturate_cast(i*180./hsize), 255, 255); + for (int i = 0; i < hsize; i++) + buf.at(i) = cv::Vec3b(cv::saturate_cast(i * 180. / hsize), 255, 255); cv::cvtColor(buf, buf, cv::COLOR_HSV2BGR); - - for( int i = 0; i < hsize; i++ ) + + for (int i = 0; i < hsize; i++) { - int val = cv::saturate_cast(hist.at(i)*histimg.rows/255); - cv::rectangle( histimg, cv::Point(i*binW,histimg.rows), - cv::Point((i+1)*binW,histimg.rows - val), - cv::Scalar(buf.at(i)), -1, 8 ); + int val = cv::saturate_cast(hist.at(i) * histimg.rows / 255); + cv::rectangle(histimg, cv::Point(i * binW, histimg.rows), cv::Point((i + 1) * binW, histimg.rows - val), + cv::Scalar(buf.at(i)), -1, 8); } } onInitPostProcess(); @@ -429,19 +437,21 @@ bool CamShiftNodelet::on_mouse_update_ = false; int CamShiftNodelet::on_mouse_event_ = 0; int CamShiftNodelet::on_mouse_x_ = 0; int CamShiftNodelet::on_mouse_y_ = 0; -} // namespace opencv_apps +} // namespace opencv_apps -namespace camshift { -class CamShiftNodelet : public opencv_apps::CamShiftNodelet { +namespace camshift +{ +class CamShiftNodelet : public opencv_apps::CamShiftNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet camshift/camshift is deprecated, " "and renamed to opencv_apps/camshift."); opencv_apps::CamShiftNodelet::onInit(); } }; -} // namespace camshift - +} // namespace camshift #include PLUGINLIB_EXPORT_CLASS(opencv_apps::CamShiftNodelet, nodelet::Nodelet); diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp index 74950aea..1f796ac0 100644 --- a/src/nodelet/color_filter_nodelet.cpp +++ b/src/nodelet/color_filter_nodelet.cpp @@ -46,13 +46,15 @@ #include "opencv_apps/HLSColorFilterConfig.h" #include "opencv_apps/HSVColorFilterConfig.h" -namespace color_filter { +namespace color_filter +{ class RGBColorFilterNodelet; class HLSColorFilterNodelet; class HSVColorFilterNodelet; } -namespace opencv_apps { +namespace opencv_apps +{ class RGBColorFilter; class HLSColorFilter; class HSVColorFilter; @@ -84,11 +86,11 @@ class ColorFilterNodelet : public opencv_apps::Nodelet boost::mutex mutex_; - virtual void reconfigureCallback(Config &new_config, uint32_t level) = 0; + virtual void reconfigureCallback(Config& new_config, uint32_t level) = 0; virtual void filter(const cv::Mat& input_image, cv::Mat& output_image) = 0; - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -109,38 +111,42 @@ class ColorFilterNodelet : public opencv_apps::Nodelet { // Work on the image. try - { - // Convert the image into something opencv can handle. - cv::Mat frame = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8)->image; + { + // Convert the image into something opencv can handle. + cv::Mat frame = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8)->image; - // Do the work - cv::Mat out_frame; - filter(frame, out_frame); + // Do the work + cv::Mat out_frame; + filter(frame, out_frame); - /// Create window - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); - } - - std::string new_window_name; + /// Create window + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + } - if( debug_view_) { - if (window_name_ != new_window_name) { - cv::destroyWindow(window_name_); - window_name_ = new_window_name; - } - cv::imshow( window_name_, out_frame ); - int c = cv::waitKey(1); - } + std::string new_window_name; - // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::MONO8, out_frame).toImageMsg(); - img_pub_.publish(out_img); - } - catch (cv::Exception &e) + if (debug_view_) { - NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); + if (window_name_ != new_window_name) + { + cv::destroyWindow(window_name_); + window_name_ = new_window_name; + } + cv::imshow(window_name_, out_frame); + int c = cv::waitKey(1); } + + // Publish the image. + sensor_msgs::Image::Ptr out_img = + cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::MONO8, out_frame).toImageMsg(); + img_pub_.publish(out_img); + } + catch (cv::Exception& e) + { + NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); + } } void subscribe() { @@ -167,7 +173,8 @@ class ColorFilterNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } @@ -175,7 +182,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); typename dynamic_reconfigure::Server::CallbackType f = - boost::bind(&ColorFilterNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&ColorFilterNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -184,8 +191,8 @@ class ColorFilterNodelet : public opencv_apps::Nodelet } }; -class RGBColorFilterNodelet - : public ColorFilterNodelet { +class RGBColorFilterNodelet : public ColorFilterNodelet +{ protected: int r_min_; int r_max_; @@ -194,8 +201,8 @@ class RGBColorFilterNodelet int g_min_; int g_max_; - virtual void reconfigureCallback(opencv_apps::RGBColorFilterConfig& config, - uint32_t level) { + virtual void reconfigureCallback(opencv_apps::RGBColorFilterConfig& config, uint32_t level) + { boost::mutex::scoped_lock lock(mutex_); config_ = config; r_max_ = config.r_limit_max; @@ -207,21 +214,26 @@ class RGBColorFilterNodelet updateCondition(); } - virtual void updateCondition() { - if (r_max_ < r_min_) std::swap(r_max_, r_min_); - if (g_max_ < g_min_) std::swap(g_max_, g_min_); - if (b_max_ < b_min_) std::swap(b_max_, b_min_); + virtual void updateCondition() + { + if (r_max_ < r_min_) + std::swap(r_max_, r_min_); + if (g_max_ < g_min_) + std::swap(g_max_, g_min_); + if (b_max_ < b_min_) + std::swap(b_max_, b_min_); lower_color_range_ = cv::Scalar(b_min_, g_min_, r_min_); upper_color_range_ = cv::Scalar(b_max_, g_max_, r_max_); } - virtual void filter(const cv::Mat& input_image, cv::Mat& output_image) { - cv::inRange(input_image, lower_color_range_, upper_color_range_, - output_image); + virtual void filter(const cv::Mat& input_image, cv::Mat& output_image) + { + cv::inRange(input_image, lower_color_range_, upper_color_range_, output_image); } protected: - virtual void onInit() { + virtual void onInit() + { r_max_ = 255; r_min_ = 0; g_max_ = 255; @@ -233,11 +245,11 @@ class RGBColorFilterNodelet } friend class color_filter::RGBColorFilterNodelet; friend class color_filter::HLSColorFilterNodelet; - friend class color_filter::HSVColorFilterNodelet; + friend class color_filter::HSVColorFilterNodelet; }; -class HLSColorFilterNodelet - : public ColorFilterNodelet { +class HLSColorFilterNodelet : public ColorFilterNodelet +{ protected: int h_min_; int h_max_; @@ -246,8 +258,8 @@ class HLSColorFilterNodelet int l_min_; int l_max_; - virtual void reconfigureCallback(opencv_apps::HLSColorFilterConfig& config, - uint32_t level) { + virtual void reconfigureCallback(opencv_apps::HLSColorFilterConfig& config, uint32_t level) + { boost::mutex::scoped_lock lock(mutex_); config_ = config; h_max_ = config.h_limit_max; @@ -259,24 +271,30 @@ class HLSColorFilterNodelet updateCondition(); } - virtual void updateCondition() { - if (s_max_ < s_min_) std::swap(s_max_, s_min_); - if (l_max_ < l_min_) std::swap(l_max_, l_min_); - lower_color_range_ = cv::Scalar(h_min_/2, l_min_, s_min_, 0); - upper_color_range_ = cv::Scalar(h_max_/2, l_max_, s_max_, 0); + virtual void updateCondition() + { + if (s_max_ < s_min_) + std::swap(s_max_, s_min_); + if (l_max_ < l_min_) + std::swap(l_max_, l_min_); + lower_color_range_ = cv::Scalar(h_min_ / 2, l_min_, s_min_, 0); + upper_color_range_ = cv::Scalar(h_max_ / 2, l_max_, s_max_, 0); } - virtual void filter(const cv::Mat& input_image, cv::Mat& output_image) { + virtual void filter(const cv::Mat& input_image, cv::Mat& output_image) + { cv::Mat hls_image; cv::cvtColor(input_image, hls_image, cv::COLOR_BGR2HLS); - if ( lower_color_range_[0] < upper_color_range_[0] ) { - cv::inRange(hls_image, lower_color_range_, upper_color_range_, - output_image); - }else { - cv::Scalar lower_color_range_0 = cv::Scalar( 0, l_min_, s_min_, 0); - cv::Scalar upper_color_range_0 = cv::Scalar(h_max_/2, l_max_, s_max_, 0); - cv::Scalar lower_color_range_360 = cv::Scalar(h_min_/2, l_min_, s_min_, 0); - cv::Scalar upper_color_range_360 = cv::Scalar( 360/2, l_max_, s_max_, 0); + if (lower_color_range_[0] < upper_color_range_[0]) + { + cv::inRange(hls_image, lower_color_range_, upper_color_range_, output_image); + } + else + { + cv::Scalar lower_color_range_0 = cv::Scalar(0, l_min_, s_min_, 0); + cv::Scalar upper_color_range_0 = cv::Scalar(h_max_ / 2, l_max_, s_max_, 0); + cv::Scalar lower_color_range_360 = cv::Scalar(h_min_ / 2, l_min_, s_min_, 0); + cv::Scalar upper_color_range_360 = cv::Scalar(360 / 2, l_max_, s_max_, 0); cv::Mat output_image_0, output_image_360; cv::inRange(hls_image, lower_color_range_0, upper_color_range_0, output_image_0); cv::inRange(hls_image, lower_color_range_360, upper_color_range_360, output_image_360); @@ -285,7 +303,8 @@ class HLSColorFilterNodelet } public: - virtual void onInit() { + virtual void onInit() + { h_max_ = 360; h_min_ = 0; s_max_ = 256; @@ -297,8 +316,8 @@ class HLSColorFilterNodelet } }; -class HSVColorFilterNodelet - : public ColorFilterNodelet { +class HSVColorFilterNodelet : public ColorFilterNodelet +{ protected: int h_min_; int h_max_; @@ -307,8 +326,8 @@ class HSVColorFilterNodelet int v_min_; int v_max_; - virtual void reconfigureCallback(opencv_apps::HSVColorFilterConfig& config, - uint32_t level) { + virtual void reconfigureCallback(opencv_apps::HSVColorFilterConfig& config, uint32_t level) + { boost::mutex::scoped_lock lock(mutex_); config_ = config; h_max_ = config.h_limit_max; @@ -320,24 +339,30 @@ class HSVColorFilterNodelet updateCondition(); } - virtual void updateCondition() { - if (s_max_ < s_min_) std::swap(s_max_, s_min_); - if (v_max_ < v_min_) std::swap(v_max_, v_min_); - lower_color_range_ = cv::Scalar(h_min_/2, s_min_, v_min_, 0); - upper_color_range_ = cv::Scalar(h_max_/2, s_max_, v_max_, 0); + virtual void updateCondition() + { + if (s_max_ < s_min_) + std::swap(s_max_, s_min_); + if (v_max_ < v_min_) + std::swap(v_max_, v_min_); + lower_color_range_ = cv::Scalar(h_min_ / 2, s_min_, v_min_, 0); + upper_color_range_ = cv::Scalar(h_max_ / 2, s_max_, v_max_, 0); } - virtual void filter(const cv::Mat& input_image, cv::Mat& output_image) { + virtual void filter(const cv::Mat& input_image, cv::Mat& output_image) + { cv::Mat hsv_image; cv::cvtColor(input_image, hsv_image, cv::COLOR_BGR2HSV); - if ( lower_color_range_[0] < upper_color_range_[0] ) { - cv::inRange(hsv_image, lower_color_range_, upper_color_range_, - output_image); - }else { - cv::Scalar lower_color_range_0 = cv::Scalar( 0, s_min_, v_min_, 0); - cv::Scalar upper_color_range_0 = cv::Scalar(h_max_/2, s_max_, v_max_, 0); - cv::Scalar lower_color_range_360 = cv::Scalar(h_min_/2, s_min_, v_min_, 0); - cv::Scalar upper_color_range_360 = cv::Scalar( 360/2, s_max_, v_max_, 0); + if (lower_color_range_[0] < upper_color_range_[0]) + { + cv::inRange(hsv_image, lower_color_range_, upper_color_range_, output_image); + } + else + { + cv::Scalar lower_color_range_0 = cv::Scalar(0, s_min_, v_min_, 0); + cv::Scalar upper_color_range_0 = cv::Scalar(h_max_ / 2, s_max_, v_max_, 0); + cv::Scalar lower_color_range_360 = cv::Scalar(h_min_ / 2, s_min_, v_min_, 0); + cv::Scalar upper_color_range_360 = cv::Scalar(360 / 2, s_max_, v_max_, 0); cv::Mat output_image_0, output_image_360; cv::inRange(hsv_image, lower_color_range_0, upper_color_range_0, output_image_0); cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360); @@ -346,7 +371,8 @@ class HSVColorFilterNodelet } public: - virtual void onInit() { + virtual void onInit() + { h_max_ = 360; h_min_ = 0; s_max_ = 256; @@ -358,35 +384,41 @@ class HSVColorFilterNodelet } }; -} // namespace opencv_apps +} // namespace opencv_apps -namespace color_filter { -class RGBColorFilterNodelet : public opencv_apps::RGBColorFilterNodelet { +namespace color_filter +{ +class RGBColorFilterNodelet : public opencv_apps::RGBColorFilterNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet rgb_color_filter/rgb_color_filter is deprecated, " "and renamed to opencv_apps/rgb_color_filter."); opencv_apps::RGBColorFilterNodelet::onInit(); } }; -class HLSColorFilterNodelet : public opencv_apps::HLSColorFilterNodelet { +class HLSColorFilterNodelet : public opencv_apps::HLSColorFilterNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet hls_color_filter/hls_color_filter is deprecated, " "and renamed to opencv_apps/hls_color_filter."); opencv_apps::HLSColorFilterNodelet::onInit(); } }; -class HSVColorFilterNodelet : public opencv_apps::HSVColorFilterNodelet { +class HSVColorFilterNodelet : public opencv_apps::HSVColorFilterNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet hsv_color_filter/hsv_color_filter is deprecated, " "and renamed to opencv_apps/hsv_color_filter."); opencv_apps::HSVColorFilterNodelet::onInit(); } }; -} // namespace color_filter - +} // namespace color_filter #include PLUGINLIB_EXPORT_CLASS(color_filter::RGBColorFilterNodelet, nodelet::Nodelet); diff --git a/src/nodelet/contour_moments_nodelet.cpp b/src/nodelet/contour_moments_nodelet.cpp index 482f93c2..e4dcc2c9 100644 --- a/src/nodelet/contour_moments_nodelet.cpp +++ b/src/nodelet/contour_moments_nodelet.cpp @@ -54,14 +54,15 @@ #include "opencv_apps/MomentArray.h" #include "opencv_apps/MomentArrayStamped.h" -namespace opencv_apps { - +namespace opencv_apps +{ // https://stackoverflow.com/questions/13495207/opencv-c-sorting-contours-by-their-contourarea // comparison function object -bool compareContourAreas ( std::vector contour1, std::vector contour2 ) { - double i = fabs( contourArea(cv::Mat(contour1)) ); - double j = fabs( contourArea(cv::Mat(contour2)) ); - return ( i > j ); +bool compareContourAreas(std::vector contour1, std::vector contour2) +{ + double i = fabs(contourArea(cv::Mat(contour1))); + double j = fabs(contourArea(cv::Mat(contour2))); + return (i > j); } class ContourMomentsNodelet : public opencv_apps::Nodelet @@ -87,13 +88,13 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet std::string window_name_; static bool need_config_update_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; low_threshold_ = config_.canny_low_threshold; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -110,7 +111,7 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -130,16 +131,20 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet // Do the work cv::Mat src_gray; /// Convert image to gray and blur it - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, src_gray, cv::COLOR_BGR2GRAY); + } + else + { src_gray = frame; } - cv::blur( src_gray, src_gray, cv::Size(3,3) ); + cv::blur(src_gray, src_gray, cv::Size(3, 3)); /// Create window - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); } cv::Mat canny_output; @@ -148,14 +153,15 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet cv::RNG rng(12345); /// Detect edges using canny - cv::Canny( src_gray, canny_output, low_threshold_ , low_threshold_ *2, 3 ); + cv::Canny(src_gray, canny_output, low_threshold_, low_threshold_ * 2, 3); /// Find contours - cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) ); + cv::findContours(canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0)); /// Draw contours cv::Mat drawing; - if( debug_view_) { - drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 ); + if (debug_view_) + { + drawing = cv::Mat::zeros(canny_output.size(), CV_8UC3); } /// Calculate the area with the moments 00 and compare with the result of the OpenCV function @@ -164,24 +170,31 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet // https://stackoverflow.com/questions/13495207/opencv-c-sorting-contours-by-their-contourarea std::sort(contours.begin(), contours.end(), compareContourAreas); - std::vector mu(contours.size() ); - std::vector mc( contours.size() ); - for( size_t i = 0; i< contours.size(); i++ ) + std::vector mu(contours.size()); + std::vector mc(contours.size()); + for (size_t i = 0; i < contours.size(); i++) { /// Get the moments - for( size_t i = 0; i < contours.size(); i++ ) - { mu[i] = moments( contours[i], false ); } + for (size_t i = 0; i < contours.size(); i++) + { + mu[i] = moments(contours[i], false); + } /// Get the mass centers: - for( size_t i = 0; i < contours.size(); i++ ) - { mc[i] = cv::Point2f( static_cast(mu[i].m10/mu[i].m00) , static_cast(mu[i].m01/mu[i].m00) ); } + for (size_t i = 0; i < contours.size(); i++) + { + mc[i] = cv::Point2f(static_cast(mu[i].m10 / mu[i].m00), static_cast(mu[i].m01 / mu[i].m00)); + } - if( debug_view_) { - cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ); - cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() ); - cv::circle( drawing, mc[i], 4, color, -1, 8, 0 ); + if (debug_view_) + { + cv::Scalar color = cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)); + cv::drawContours(drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point()); + cv::circle(drawing, mc[i], 4, color, -1, 8, 0); } - NODELET_INFO(" * Contour[%d] - Area (M_00) = %.2f - Area OpenCV: %.2f - Length: %.2f - Center (%.2f, %.2f)", (int)i, mu[i].m00, cv::contourArea(contours[i]), cv::arcLength( contours[i], true ), mc[i].x, mc[i].y ); + NODELET_INFO(" * Contour[%d] - Area (M_00) = %.2f - Area OpenCV: %.2f - Length: %.2f - Center (%.2f, %.2f)", + (int)i, mu[i].m00, cv::contourArea(contours[i]), cv::arcLength(contours[i], true), mc[i].x, + mc[i].y); opencv_apps::Moment moment_msg; moment_msg.m00 = mu[i].m00; @@ -217,8 +230,9 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet moments_msg.moments.push_back(moment_msg); } - if( debug_view_) { - cv::imshow( window_name_, drawing ); + if (debug_view_) + { + cv::imshow(window_name_, drawing); int c = cv::waitKey(1); } @@ -227,7 +241,7 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet img_pub_.publish(out_img); msg_pub_.publish(moments_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -259,17 +273,18 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Contours"; - low_threshold_ = 100; // only for canny - + low_threshold_ = 100; // only for canny + reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&ContourMomentsNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&ContourMomentsNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -278,19 +293,21 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet } }; bool ContourMomentsNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace contour_moments { -class ContourMomentsNodelet : public opencv_apps::ContourMomentsNodelet { +namespace contour_moments +{ +class ContourMomentsNodelet : public opencv_apps::ContourMomentsNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet contour_moments/contour_moments is deprecated, " "and renamed to opencv_apps/contour_moments."); opencv_apps::ContourMomentsNodelet::onInit(); } }; -} // namespace contour_moments - +} // namespace contour_moments #include PLUGINLIB_EXPORT_CLASS(opencv_apps::ContourMomentsNodelet, nodelet::Nodelet); diff --git a/src/nodelet/convex_hull_nodelet.cpp b/src/nodelet/convex_hull_nodelet.cpp index 56f3f2d0..4e8dd659 100644 --- a/src/nodelet/convex_hull_nodelet.cpp +++ b/src/nodelet/convex_hull_nodelet.cpp @@ -55,7 +55,8 @@ #include "opencv_apps/ContourArray.h" #include "opencv_apps/ContourArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class ConvexHullNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -79,13 +80,13 @@ class ConvexHullNodelet : public opencv_apps::Nodelet std::string window_name_; static bool need_config_update_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; threshold_ = config_.threshold; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -102,7 +103,7 @@ class ConvexHullNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -123,16 +124,20 @@ class ConvexHullNodelet : public opencv_apps::Nodelet cv::Mat src_gray; /// Convert image to gray and blur it - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, src_gray, cv::COLOR_RGB2GRAY); + } + else + { src_gray = frame; } - cv::blur( src_gray, src_gray, cv::Size(3,3) ); + cv::blur(src_gray, src_gray, cv::Size(3, 3)); /// Create window - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); } cv::Mat threshold_output; @@ -142,26 +147,29 @@ class ConvexHullNodelet : public opencv_apps::Nodelet cv::RNG rng(12345); /// Detect edges using Threshold - cv::threshold( src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY ); + cv::threshold(src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY); /// Find contours - cv::findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) ); + cv::findContours(threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0)); /// Find the convex hull object for each contour - std::vector >hull( contours.size() ); - for( size_t i = 0; i < contours.size(); i++ ) - { cv::convexHull( cv::Mat(contours[i]), hull[i], false ); } + std::vector > hull(contours.size()); + for (size_t i = 0; i < contours.size(); i++) + { + cv::convexHull(cv::Mat(contours[i]), hull[i], false); + } /// Draw contours + hull results - cv::Mat drawing = cv::Mat::zeros( threshold_output.size(), CV_8UC3 ); - for( size_t i = 0; i< contours.size(); i++ ) + cv::Mat drawing = cv::Mat::zeros(threshold_output.size(), CV_8UC3); + for (size_t i = 0; i < contours.size(); i++) { - cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ); - cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector(), 0, cv::Point() ); - cv::drawContours( drawing, hull, (int)i, color, 4, 8, std::vector(), 0, cv::Point() ); + cv::Scalar color = cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)); + cv::drawContours(drawing, contours, (int)i, color, 1, 8, std::vector(), 0, cv::Point()); + cv::drawContours(drawing, hull, (int)i, color, 4, 8, std::vector(), 0, cv::Point()); opencv_apps::Contour contour_msg; - for ( size_t j = 0; j < hull[i].size(); j++ ) { + for (size_t j = 0; j < hull[i].size(); j++) + { opencv_apps::Point2D point_msg; point_msg.x = hull[i][j].x; point_msg.y = hull[i][j].y; @@ -171,24 +179,27 @@ class ConvexHullNodelet : public opencv_apps::Nodelet } /// Create a Trackbar for user to enter threshold - if( debug_view_) { - if (need_config_update_) { + if (debug_view_) + { + if (need_config_update_) + { config_.threshold = threshold_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } - cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback); + cv::createTrackbar("Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback); - cv::imshow( window_name_, drawing ); + cv::imshow(window_name_, drawing); int c = cv::waitKey(1); } // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg(); + sensor_msgs::Image::Ptr out_img = + cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(contours_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -212,7 +223,6 @@ class ConvexHullNodelet : public opencv_apps::Nodelet cam_sub_.shutdown(); } - public: virtual void onInit() { @@ -221,17 +231,18 @@ class ConvexHullNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Hull Demo"; threshold_ = 100; - + reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&ConvexHullNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&ConvexHullNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -240,19 +251,21 @@ class ConvexHullNodelet : public opencv_apps::Nodelet } }; bool ConvexHullNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace convex_hull { -class ConvexHullNodelet : public opencv_apps::ConvexHullNodelet { +namespace convex_hull +{ +class ConvexHullNodelet : public opencv_apps::ConvexHullNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet convex_hull/convex_hull is deprecated, " "and renamed to opencv_apps/convex_hull."); opencv_apps::ConvexHullNodelet::onInit(); } }; -} // namespace convex_hull - +} // namespace convex_hull #include PLUGINLIB_EXPORT_CLASS(opencv_apps::ConvexHullNodelet, nodelet::Nodelet); diff --git a/src/nodelet/corner_harris_nodelet.cpp b/src/nodelet/corner_harris_nodelet.cpp index a18eb3f8..eec4b3be 100644 --- a/src/nodelet/corner_harris_nodelet.cpp +++ b/src/nodelet/corner_harris_nodelet.cpp @@ -51,7 +51,8 @@ * @author OpenCV team */ -namespace opencv_apps { +namespace opencv_apps +{ class CornerHarrisNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -74,13 +75,13 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet int threshold_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; threshold_ = config_.threshold; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -97,7 +98,7 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -112,15 +113,18 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet // Do the work cv::Mat dst, dst_norm, dst_norm_scaled; - dst = cv::Mat::zeros( frame.size(), CV_32FC1 ); + dst = cv::Mat::zeros(frame.size(), CV_32FC1); cv::Mat src_gray; - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, src_gray, cv::COLOR_BGR2GRAY); + } + else + { src_gray = frame; - cv::cvtColor( src_gray, frame, cv::COLOR_GRAY2BGR ); + cv::cvtColor(src_gray, frame, cv::COLOR_GRAY2BGR); } /// Detector parameters @@ -129,43 +133,50 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet double k = 0.04; /// Detecting corners - cv::cornerHarris( src_gray, dst, blockSize, apertureSize, k, cv::BORDER_DEFAULT ); + cv::cornerHarris(src_gray, dst, blockSize, apertureSize, k, cv::BORDER_DEFAULT); /// Normalizing - cv::normalize( dst, dst_norm, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat() ); - cv::convertScaleAbs( dst_norm, dst_norm_scaled ); + cv::normalize(dst, dst_norm, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat()); + cv::convertScaleAbs(dst_norm, dst_norm_scaled); /// Drawing a circle around corners - for( int j = 0; j < dst_norm.rows ; j++ ) { - for( int i = 0; i < dst_norm.cols; i++ ) { - if( (int) dst_norm.at(j,i) > threshold_ ) { - cv::circle( frame, cv::Point(i, j), 5, cv::Scalar(0), 2, 8, 0 ); + for (int j = 0; j < dst_norm.rows; j++) + { + for (int i = 0; i < dst_norm.cols; i++) + { + if ((int)dst_norm.at(j, i) > threshold_) + { + cv::circle(frame, cv::Point(i, j), 5, cv::Scalar(0), 2, 8, 0); } } } /// Create window - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); const int max_threshold = 255; - if (need_config_update_) { + if (need_config_update_) + { config_.threshold = threshold_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } - cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_threshold, trackbarCallback); + cv::createTrackbar("Threshold:", window_name_, &threshold_, max_threshold, trackbarCallback); } - if( debug_view_) { - cv::imshow( window_name_, frame ); + if (debug_view_) + { + cv::imshow(window_name_, frame); int c = cv::waitKey(1); } // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg(); + sensor_msgs::Image::Ptr out_img = + cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg(); img_pub_.publish(out_img); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -196,7 +207,8 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } @@ -204,7 +216,7 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&CornerHarrisNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&CornerHarrisNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -213,19 +225,21 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet } }; bool CornerHarrisNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace corner_harris { -class CornerHarrisNodelet : public opencv_apps::CornerHarrisNodelet { +namespace corner_harris +{ +class CornerHarrisNodelet : public opencv_apps::CornerHarrisNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet corner_harris/corner_harris is deprecated, " "and renamed to opencv_apps/corner_harris."); opencv_apps::CornerHarrisNodelet::onInit(); } }; -} // namespace corner_harris - +} // namespace corner_harris #include PLUGINLIB_EXPORT_CLASS(opencv_apps::CornerHarrisNodelet, nodelet::Nodelet); diff --git a/src/nodelet/discrete_fourier_transform_nodelet.cpp b/src/nodelet/discrete_fourier_transform_nodelet.cpp index 1dcd62fb..0ca2ad07 100644 --- a/src/nodelet/discrete_fourier_transform_nodelet.cpp +++ b/src/nodelet/discrete_fourier_transform_nodelet.cpp @@ -50,8 +50,10 @@ #include -namespace opencv_apps { -class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet { +namespace opencv_apps +{ +class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet +{ image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; @@ -71,104 +73,111 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet { std::string window_name_; - void imageCallbackWithInfo( - const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::CameraInfoConstPtr& cam_info) { + void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) + { do_work(msg, cam_info->header.frame_id); } - void imageCallback(const sensor_msgs::ImageConstPtr& msg) { + void imageCallback(const sensor_msgs::ImageConstPtr& msg) + { do_work(msg, msg->header.frame_id); } - void subscribe() { + void subscribe() + { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera( - "image", queue_size_, &DiscreteFourierTransformNodelet::imageCallbackWithInfo, this); + cam_sub_ = + it_->subscribeCamera("image", queue_size_, &DiscreteFourierTransformNodelet::imageCallbackWithInfo, this); else - img_sub_ = - it_->subscribe("image", queue_size_, &DiscreteFourierTransformNodelet::imageCallback, this); + img_sub_ = it_->subscribe("image", queue_size_, &DiscreteFourierTransformNodelet::imageCallback, this); } - void unsubscribe() { + void unsubscribe() + { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); cam_sub_.shutdown(); } - void reconfigureCallback(Config& config, uint32_t level) { + void reconfigureCallback(Config& config, uint32_t level) + { boost::mutex::scoped_lock lock(mutex_); config_ = config; } - void do_work(const sensor_msgs::Image::ConstPtr& image_msg, - const std::string input_frame_from_msg) { + void do_work(const sensor_msgs::Image::ConstPtr& image_msg, const std::string input_frame_from_msg) + { // Work on the image. - try { - cv::Mat src_image = - cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; - if(src_image.channels() > 1) { - cv::cvtColor(src_image, src_image, CV_BGR2GRAY); + try + { + cv::Mat src_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; + if (src_image.channels() > 1) + { + cv::cvtColor(src_image, src_image, CV_BGR2GRAY); + } + + cv::Mat padded; // expand input image to optimal size + int m = cv::getOptimalDFTSize(src_image.rows); + int n = cv::getOptimalDFTSize(src_image.cols); // on the border add zero values + cv::copyMakeBorder(src_image, padded, 0, m - src_image.rows, 0, n - src_image.cols, cv::BORDER_CONSTANT, + cv::Scalar::all(0)); + + cv::Mat planes[] = { cv::Mat_(padded), cv::Mat::zeros(padded.size(), CV_32F) }; + cv::Mat complex_image; + cv::merge(planes, 2, complex_image); // Add to the expanded another plane with zeros + cv::dft(complex_image, complex_image); // this way the result may fit in the source matrix + + // compute the magnitude and switch to logarithmic scale + // => log(1 + sqrt(Re(DFT(I))^2 + Im(DFT(I))^2)) + cv::split(complex_image, planes); // planes[0] = Re(DFT(I), planes[1] = Im(DFT(I)) + cv::magnitude(planes[0], planes[1], planes[0]); // planes[0] = magnitude + cv::Mat magnitude_image = planes[0]; + magnitude_image += cv::Scalar::all(1); // switch to logarithmic scale + cv::log(magnitude_image, magnitude_image); + + // crop the spectrum, if it has an odd number of rows or columns + magnitude_image = magnitude_image(cv::Rect(0, 0, magnitude_image.cols & -2, magnitude_image.rows & -2)); + // rearrange the quadrants of Fourier imagev so that the origin is at the image center + int cx = magnitude_image.cols / 2; + int cy = magnitude_image.rows / 2; + + cv::Mat q0(magnitude_image, cv::Rect(0, 0, cx, cy)); // Top-Left - Create a ROI per quadrant + cv::Mat q1(magnitude_image, cv::Rect(cx, 0, cx, cy)); // Top-Right + cv::Mat q2(magnitude_image, cv::Rect(0, cy, cx, cy)); // Bottom-Left + cv::Mat q3(magnitude_image, cv::Rect(cx, cy, cx, cy)); // Bottom-Right + + cv::Mat tmp; // swap quadrants (Top-Left with Bottom-Right) + q0.copyTo(tmp); + q3.copyTo(q0); + tmp.copyTo(q3); + + q1.copyTo(tmp); // swap quadrant (Top-Right with Bottom-Left) + q2.copyTo(q1); + tmp.copyTo(q2); + cv::normalize(magnitude_image, magnitude_image, 0, 255, cv::NORM_MINMAX); + + cv::Mat result_image = cv::Mat::zeros(magnitude_image.rows, magnitude_image.cols, CV_8UC1); + for (int i = 0; i < magnitude_image.rows; ++i) + { + unsigned char* result_data = result_image.ptr(i); + float* magnitude_data = magnitude_image.ptr(i); + for (int j = 0; j < magnitude_image.cols; ++j) + { + *result_data++ = (unsigned char)(*magnitude_data++); } - - cv::Mat padded; //expand input image to optimal size - int m = cv::getOptimalDFTSize(src_image.rows); - int n = cv::getOptimalDFTSize(src_image.cols); // on the border add zero values - cv::copyMakeBorder(src_image, padded, 0, m - src_image.rows, 0, n - src_image.cols, cv::BORDER_CONSTANT, cv::Scalar::all(0)); - - cv::Mat planes[] = {cv::Mat_(padded), cv::Mat::zeros(padded.size(), CV_32F)}; - cv::Mat complex_image; - cv::merge(planes, 2, complex_image); // Add to the expanded another plane with zeros - cv::dft(complex_image, complex_image); // this way the result may fit in the source matrix - - // compute the magnitude and switch to logarithmic scale - // => log(1 + sqrt(Re(DFT(I))^2 + Im(DFT(I))^2)) - cv::split(complex_image, planes); // planes[0] = Re(DFT(I), planes[1] = Im(DFT(I)) - cv::magnitude(planes[0], planes[1], planes[0]);// planes[0] = magnitude - cv::Mat magnitude_image = planes[0]; - magnitude_image += cv::Scalar::all(1); // switch to logarithmic scale - cv::log(magnitude_image, magnitude_image); - - // crop the spectrum, if it has an odd number of rows or columns - magnitude_image = magnitude_image(cv::Rect(0, 0, magnitude_image.cols & -2, magnitude_image.rows & -2)); - // rearrange the quadrants of Fourier imagev so that the origin is at the image center - int cx = magnitude_image.cols / 2; - int cy = magnitude_image.rows / 2; - - cv::Mat q0(magnitude_image, cv::Rect(0, 0, cx, cy)); // Top-Left - Create a ROI per quadrant - cv::Mat q1(magnitude_image, cv::Rect(cx, 0, cx, cy)); // Top-Right - cv::Mat q2(magnitude_image, cv::Rect(0, cy, cx, cy)); // Bottom-Left - cv::Mat q3(magnitude_image, cv::Rect(cx, cy, cx, cy)); // Bottom-Right - - cv::Mat tmp; // swap quadrants (Top-Left with Bottom-Right) - q0.copyTo(tmp); - q3.copyTo(q0); - tmp.copyTo(q3); - - q1.copyTo(tmp); // swap quadrant (Top-Right with Bottom-Left) - q2.copyTo(q1); - tmp.copyTo(q2); - cv::normalize(magnitude_image, magnitude_image, 0, 255, cv::NORM_MINMAX); - - cv::Mat result_image = cv::Mat::zeros(magnitude_image.rows, magnitude_image.cols, CV_8UC1); - for(int i=0;i(i); - float* magnitude_data = magnitude_image.ptr(i); - for(int j=0;jheader, - sensor_msgs::image_encodings::MONO8, - result_image).toImageMsg()); + } + + if (debug_view_) + { + cv::imshow(window_name_, result_image); + int c = cv::waitKey(1); + } + img_pub_.publish( + cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::MONO8, result_image).toImageMsg()); } - catch (cv::Exception &e) { + catch (cv::Exception& e) + { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -176,13 +185,15 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet { } public: - virtual void onInit() { + virtual void onInit() + { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("queue_size", queue_size_, 1); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -191,26 +202,28 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet { reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&DiscreteFourierTransformNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&DiscreteFourierTransformNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); onInitPostProcess(); } }; -} // namespace opencv_apps +} // namespace opencv_apps -namespace discrete_fourier_transform { -class DiscreteFourierTransformNodelet : public opencv_apps::DiscreteFourierTransformNodelet { +namespace discrete_fourier_transform +{ +class DiscreteFourierTransformNodelet : public opencv_apps::DiscreteFourierTransformNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet discrete_fourier_transform/discrete_fourier_transform is deprecated, " "and renamed to opencv_apps/discrete_fourier_transform."); opencv_apps::DiscreteFourierTransformNodelet::onInit(); } }; -} // namespace discrete_fourier_transform - +} // namespace discrete_fourier_transform #include PLUGINLIB_EXPORT_CLASS(opencv_apps::DiscreteFourierTransformNodelet, nodelet::Nodelet); diff --git a/src/nodelet/edge_detection_nodelet.cpp b/src/nodelet/edge_detection_nodelet.cpp index ceb9864b..abdc348e 100644 --- a/src/nodelet/edge_detection_nodelet.cpp +++ b/src/nodelet/edge_detection_nodelet.cpp @@ -62,7 +62,8 @@ #include #include "opencv_apps/EdgeDetectionConfig.h" -namespace opencv_apps { +namespace opencv_apps +{ class EdgeDetectionNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -87,27 +88,27 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet bool L2gradient_; bool apply_blur_pre_; bool apply_blur_post_; - int postBlurSize_; - double postBlurSigma_; + int postBlurSize_; + double postBlurSigma_; std::string window_name_; static bool need_config_update_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; canny_threshold1_ = config_.canny_threshold1; canny_threshold2_ = config_.canny_threshold2; - apertureSize_ = 2*((config_.apertureSize/2)) + 1; + apertureSize_ = 2 * ((config_.apertureSize / 2)) + 1; L2gradient_ = config_.L2gradient; - apply_blur_pre_ = config_.apply_blur_pre; + apply_blur_pre_ = config_.apply_blur_pre; apply_blur_post_ = config_.apply_blur_post; - postBlurSize_ = 2*((config_.postBlurSize)/2) + 1; - postBlurSigma_ = config_.postBlurSigma; + postBlurSize_ = 2 * ((config_.postBlurSize) / 2) + 1; + postBlurSigma_ = config_.postBlurSigma; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -124,7 +125,7 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -139,118 +140,132 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet // Do the work cv::Mat src_gray; - cv::GaussianBlur( frame, frame, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT ); + cv::GaussianBlur(frame, frame, cv::Size(3, 3), 0, 0, cv::BORDER_DEFAULT); /// Convert it to gray - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, src_gray, cv::COLOR_RGB2GRAY); + } + else + { src_gray = frame; } /// Create window - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); } std::string new_window_name; cv::Mat grad; - switch (config_.edge_type) { + switch (config_.edge_type) + { case opencv_apps::EdgeDetection_Sobel: - { - /// Generate grad_x and grad_y - cv::Mat grad_x, grad_y; - cv::Mat abs_grad_x, abs_grad_y; - - int scale = 1; - int delta = 0; - int ddepth = CV_16S; - - /// Gradient X - //Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT ); - cv::Sobel( src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, cv::BORDER_DEFAULT ); - cv::convertScaleAbs( grad_x, abs_grad_x ); - - /// Gradient Y - //Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT ); - cv::Sobel( src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, cv::BORDER_DEFAULT ); - cv::convertScaleAbs( grad_y, abs_grad_y ); - - /// Total Gradient (approximate) - cv::addWeighted( abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad ); - - new_window_name = "Sobel Edge Detection Demo"; - break; - } + { + /// Generate grad_x and grad_y + cv::Mat grad_x, grad_y; + cv::Mat abs_grad_x, abs_grad_y; + + int scale = 1; + int delta = 0; + int ddepth = CV_16S; + + /// Gradient X + // Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT ); + cv::Sobel(src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, cv::BORDER_DEFAULT); + cv::convertScaleAbs(grad_x, abs_grad_x); + + /// Gradient Y + // Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT ); + cv::Sobel(src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, cv::BORDER_DEFAULT); + cv::convertScaleAbs(grad_y, abs_grad_y); + + /// Total Gradient (approximate) + cv::addWeighted(abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad); + + new_window_name = "Sobel Edge Detection Demo"; + break; + } case opencv_apps::EdgeDetection_Laplace: + { + cv::Mat dst; + int kernel_size = 3; + int scale = 1; + int delta = 0; + int ddepth = CV_16S; + /// Apply Laplace function + + cv::Laplacian(src_gray, dst, ddepth, kernel_size, scale, delta, cv::BORDER_DEFAULT); + convertScaleAbs(dst, grad); + + new_window_name = "Laplace Edge Detection Demo"; + break; + } + case opencv_apps::EdgeDetection_Canny: + { + int edgeThresh = 1; + int kernel_size = 3; + int const max_canny_threshold1 = 500; + int const max_canny_threshold2 = 500; + cv::Mat detected_edges; + + /// Reduce noise with a kernel 3x3 + if (apply_blur_pre_) { - cv::Mat dst; - int kernel_size = 3; - int scale = 1; - int delta = 0; - int ddepth = CV_16S; - /// Apply Laplace function - - cv::Laplacian( src_gray, dst, ddepth, kernel_size, scale, delta, cv::BORDER_DEFAULT ); - convertScaleAbs( dst, grad ); - - new_window_name = "Laplace Edge Detection Demo"; - break; + cv::blur(src_gray, src_gray, cv::Size(apertureSize_, apertureSize_)); } - case opencv_apps::EdgeDetection_Canny: + + /// Canny detector + cv::Canny(src_gray, grad, canny_threshold1_, canny_threshold2_, kernel_size, L2gradient_); + if (apply_blur_post_) { - int edgeThresh = 1; - int kernel_size = 3; - int const max_canny_threshold1 = 500; - int const max_canny_threshold2 = 500; - cv::Mat detected_edges; - - /// Reduce noise with a kernel 3x3 - if(apply_blur_pre_) { - cv::blur( src_gray, src_gray, cv::Size(apertureSize_, apertureSize_) ); - } + cv::GaussianBlur(grad, grad, cv::Size(postBlurSize_, postBlurSize_), postBlurSigma_, + postBlurSigma_); // 0.3*(ksize/2 - 1) + 0.8 + } - /// Canny detector - cv::Canny( src_gray, grad, canny_threshold1_, canny_threshold2_, kernel_size, L2gradient_ ); - if(apply_blur_post_) { - cv::GaussianBlur(grad, grad, cv::Size(postBlurSize_, postBlurSize_), - postBlurSigma_, postBlurSigma_); // 0.3*(ksize/2 - 1) + 0.8 - } + new_window_name = "Canny Edge Detection Demo"; - new_window_name = "Canny Edge Detection Demo"; - - /// Create a Trackbar for user to enter threshold - if( debug_view_) { - if (need_config_update_) { - config_.canny_threshold1 = canny_threshold1_; - config_.canny_threshold2 = canny_threshold2_; - reconfigure_server_->updateConfig(config_); - need_config_update_ = false; - } - if( window_name_ == new_window_name) { - cv::createTrackbar( "Min CannyThreshold1:", window_name_, &canny_threshold1_, max_canny_threshold1, trackbarCallback); - cv::createTrackbar( "Min CannyThreshold2:", window_name_, &canny_threshold2_, max_canny_threshold2, trackbarCallback); - } + /// Create a Trackbar for user to enter threshold + if (debug_view_) + { + if (need_config_update_) + { + config_.canny_threshold1 = canny_threshold1_; + config_.canny_threshold2 = canny_threshold2_; + reconfigure_server_->updateConfig(config_); + need_config_update_ = false; + } + if (window_name_ == new_window_name) + { + cv::createTrackbar("Min CannyThreshold1:", window_name_, &canny_threshold1_, max_canny_threshold1, + trackbarCallback); + cv::createTrackbar("Min CannyThreshold2:", window_name_, &canny_threshold2_, max_canny_threshold2, + trackbarCallback); } - break; } + break; + } } - - if( debug_view_) { - if (window_name_ != new_window_name) { + if (debug_view_) + { + if (window_name_ != new_window_name) + { cv::destroyWindow(window_name_); window_name_ = new_window_name; } - cv::imshow( window_name_, grad ); + cv::imshow(window_name_, grad); int c = cv::waitKey(1); } // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, grad).toImageMsg(); + sensor_msgs::Image::Ptr out_img = + cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, grad).toImageMsg(); img_pub_.publish(out_img); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -283,40 +298,43 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Edge Detection Demo"; - canny_threshold1_ = 100; // only for canny - canny_threshold2_ = 200; // only for canny + canny_threshold1_ = 100; // only for canny + canny_threshold2_ = 200; // only for canny reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&EdgeDetectionNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&EdgeDetectionNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); - //msg_pub_ = local_nh_.advertise("lines", 1, msg_connect_cb, msg_disconnect_cb); + // msg_pub_ = local_nh_.advertise("lines", 1, msg_connect_cb, msg_disconnect_cb); onInitPostProcess(); } }; bool EdgeDetectionNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace edge_detection { -class EdgeDetectionNodelet : public opencv_apps::EdgeDetectionNodelet { +namespace edge_detection +{ +class EdgeDetectionNodelet : public opencv_apps::EdgeDetectionNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet edge_detection/edge_detection is deprecated, " "and renamed to opencv_apps/edge_detection."); opencv_apps::EdgeDetectionNodelet::onInit(); } }; -} // namespace edge_detection - +} // namespace edge_detection #include PLUGINLIB_EXPORT_CLASS(opencv_apps::EdgeDetectionNodelet, nodelet::Nodelet); diff --git a/src/nodelet/face_detection_nodelet.cpp b/src/nodelet/face_detection_nodelet.cpp index 73c93a17..5d0f9d84 100644 --- a/src/nodelet/face_detection_nodelet.cpp +++ b/src/nodelet/face_detection_nodelet.cpp @@ -36,7 +36,8 @@ /** * @file objectDetection.cpp * @author A. Huaman ( based in the classic facedetect.cpp in samples/c ) - * @brief A simplified version of facedetect.cpp, show how to load a cascade classifier and how to find objects (Face + eyes) in a video stream + * @brief A simplified version of facedetect.cpp, show how to load a cascade classifier and how to find objects (Face + + * eyes) in a video stream */ #include @@ -55,7 +56,8 @@ #include "opencv_apps/FaceArray.h" #include "opencv_apps/FaceArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class FaceDetectionNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -78,12 +80,12 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet cv::CascadeClassifier face_cascade_; cv::CascadeClassifier eyes_cascade_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -116,55 +118,62 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet std::vector faces; cv::Mat frame_gray; - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, frame_gray, cv::COLOR_BGR2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); + } + else + { frame_gray = frame; } - cv::equalizeHist( frame_gray, frame_gray ); - //-- Detect faces + cv::equalizeHist(frame_gray, frame_gray); +//-- Detect faces #ifndef CV_VERSION_EPOCH - face_cascade_.detectMultiScale( frame_gray, faces, 1.1, 2, 0, cv::Size(30, 30) ); + face_cascade_.detectMultiScale(frame_gray, faces, 1.1, 2, 0, cv::Size(30, 30)); #else - face_cascade_.detectMultiScale( frame_gray, faces, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) ); + face_cascade_.detectMultiScale(frame_gray, faces, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30)); #endif cv::Mat face_image; - if (faces.size() > 0) { + if (faces.size() > 0) + { cv::Rect max_area = faces[0]; - for ( size_t i = 0; i < faces.size(); i++ ) { - if (max_area.width * max_area.height > faces[i].width * faces[i].height) { + for (size_t i = 0; i < faces.size(); i++) + { + if (max_area.width * max_area.height > faces[i].width * faces[i].height) + { max_area = faces[i]; } } face_image = frame(max_area).clone(); } - for( size_t i = 0; i < faces.size(); i++ ) + for (size_t i = 0; i < faces.size(); i++) { - cv::Point center( faces[i].x + faces[i].width/2, faces[i].y + faces[i].height/2 ); - cv::ellipse( frame, center, cv::Size( faces[i].width/2, faces[i].height/2), 0, 0, 360, cv::Scalar( 255, 0, 255 ), 2, 8, 0 ); + cv::Point center(faces[i].x + faces[i].width / 2, faces[i].y + faces[i].height / 2); + cv::ellipse(frame, center, cv::Size(faces[i].width / 2, faces[i].height / 2), 0, 0, 360, + cv::Scalar(255, 0, 255), 2, 8, 0); opencv_apps::Face face_msg; face_msg.face.x = center.x; face_msg.face.y = center.y; face_msg.face.width = faces[i].width; face_msg.face.height = faces[i].height; - cv::Mat faceROI = frame_gray( faces[i] ); + cv::Mat faceROI = frame_gray(faces[i]); std::vector eyes; - //-- In each face, detect eyes +//-- In each face, detect eyes #ifndef CV_VERSION_EPOCH - eyes_cascade_.detectMultiScale( faceROI, eyes, 1.1, 2, 0, cv::Size(30, 30) ); + eyes_cascade_.detectMultiScale(faceROI, eyes, 1.1, 2, 0, cv::Size(30, 30)); #else - eyes_cascade_.detectMultiScale( faceROI, eyes, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) ); + eyes_cascade_.detectMultiScale(faceROI, eyes, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30)); #endif - for( size_t j = 0; j < eyes.size(); j++ ) + for (size_t j = 0; j < eyes.size(); j++) { - cv::Point eye_center( faces[i].x + eyes[j].x + eyes[j].width/2, faces[i].y + eyes[j].y + eyes[j].height/2 ); - int radius = cvRound( (eyes[j].width + eyes[j].height)*0.25 ); - cv::circle( frame, eye_center, radius, cv::Scalar( 255, 0, 0 ), 3, 8, 0 ); + cv::Point eye_center(faces[i].x + eyes[j].x + eyes[j].width / 2, faces[i].y + eyes[j].y + eyes[j].height / 2); + int radius = cvRound((eyes[j].width + eyes[j].height) * 0.25); + cv::circle(frame, eye_center, radius, cv::Scalar(255, 0, 0), 3, 8, 0); opencv_apps::Rect eye_msg; eye_msg.x = eye_center.x; @@ -177,21 +186,25 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet faces_msg.faces.push_back(face_msg); } //-- Show what you got - if( debug_view_) { - cv::imshow( "Face detection", frame ); + if (debug_view_) + { + cv::imshow("Face detection", frame); int c = cv::waitKey(1); } // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg(); + sensor_msgs::Image::Ptr out_img = + cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(faces_msg); - if (faces.size() > 0) { - sensor_msgs::Image::Ptr out_face_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, face_image).toImageMsg(); + if (faces.size() > 0) + { + sensor_msgs::Image::Ptr out_face_img = + cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, face_image).toImageMsg(); face_img_pub_.publish(out_face_img); } } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -223,43 +236,54 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_ ) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&FaceDetectionNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&FaceDetectionNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); - + img_pub_ = advertiseImage(*pnh_, "image", 1); face_img_pub_ = advertiseImage(*pnh_, "face_image", 1); msg_pub_ = advertise(*pnh_, "faces", 1); std::string face_cascade_name, eyes_cascade_name; - pnh_->param("face_cascade_name", face_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml")); - pnh_->param("eyes_cascade_name", eyes_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml")); + pnh_->param("face_cascade_name", face_cascade_name, + std::string("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml")); + pnh_->param("eyes_cascade_name", eyes_cascade_name, + std::string("/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml")); - if( !face_cascade_.load( face_cascade_name ) ){ NODELET_ERROR("--Error loading %s", face_cascade_name.c_str()); }; - if( !eyes_cascade_.load( eyes_cascade_name ) ){ NODELET_ERROR("--Error loading %s", eyes_cascade_name.c_str()); }; + if (!face_cascade_.load(face_cascade_name)) + { + NODELET_ERROR("--Error loading %s", face_cascade_name.c_str()); + }; + if (!eyes_cascade_.load(eyes_cascade_name)) + { + NODELET_ERROR("--Error loading %s", eyes_cascade_name.c_str()); + }; onInitPostProcess(); } }; -} // namespace opencv_apps +} // namespace opencv_apps -namespace face_detection { -class FaceDetectionNodelet : public opencv_apps::FaceDetectionNodelet { +namespace face_detection +{ +class FaceDetectionNodelet : public opencv_apps::FaceDetectionNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet face_detection/face_detection is deprecated, " "and renamed to opencv_apps/face_detection."); opencv_apps::FaceDetectionNodelet::onInit(); } }; -} // namespace face_detection - +} // namespace face_detection #include PLUGINLIB_EXPORT_CLASS(opencv_apps::FaceDetectionNodelet, nodelet::Nodelet); diff --git a/src/nodelet/face_recognition_nodelet.cpp b/src/nodelet/face_recognition_nodelet.cpp index 5e00819f..9ed51994 100644 --- a/src/nodelet/face_recognition_nodelet.cpp +++ b/src/nodelet/face_recognition_nodelet.cpp @@ -60,10 +60,9 @@ #include #include - namespace enc = sensor_msgs::image_encodings; #if BOOST_VERSION < 105000 -namespace fs = boost::filesystem3; // for hydro +namespace fs = boost::filesystem3; // for hydro #else namespace fs = boost::filesystem; #endif @@ -76,522 +75,615 @@ namespace face = cv; #endif // utility for resolving path -namespace boost { +namespace boost +{ #if BOOST_VERSION < 105000 -namespace filesystem3 { // for hydro +namespace filesystem3 +{ // for hydro #else -namespace filesystem { +namespace filesystem +{ #endif - template< > - path& path::append( - typename path::iterator lhs, typename path::iterator rhs, const codecvt_type& cvt) { - for(;lhs != rhs; ++lhs) *this /= *lhs; - return *this; +template <> +path& path::append(typename path::iterator lhs, typename path::iterator rhs, + const codecvt_type& cvt) +{ + for (; lhs != rhs; ++lhs) + *this /= *lhs; + return *this; +} +path user_expanded_path(const path& p) +{ + path::const_iterator it(p.begin()); + std::string user_dir = (*it).string(); + if (user_dir.length() == 0 || user_dir[0] != '~') + return p; + path ret; + char* homedir; + if (user_dir.length() == 1) + { + homedir = getenv("HOME"); + if (homedir == NULL) + { + homedir = getpwuid(getuid())->pw_dir; + } } - path user_expanded_path(const path &p) { - path::const_iterator it(p.begin()); - std::string user_dir = (*it).string(); - if (user_dir.length() == 0 || user_dir[0] != '~') + else + { + std::string uname = user_dir.substr(1, user_dir.length()); + passwd* pw = getpwnam(uname.c_str()); + if (pw == NULL) return p; - path ret; - char* homedir; - if (user_dir.length() == 1) { - homedir = getenv("HOME"); - if (homedir == NULL) { - homedir = getpwuid(getuid())->pw_dir; - } - } else { - std::string uname = user_dir.substr(1, user_dir.length()); - passwd* pw = getpwnam(uname.c_str()); - if (pw == NULL) return p; - homedir = pw->pw_dir; - } - ret = path(std::string(homedir)); - return ret.append(++it, p.end(), path::codecvt()); + homedir = pw->pw_dir; } -}} // end of utility for resolving paths + ret = path(std::string(homedir)); + return ret.append(++it, p.end(), path::codecvt()); +} +} +} // end of utility for resolving paths + +namespace opencv_apps +{ +class LabelMapper +{ + std::map m_; -namespace opencv_apps { - - class LabelMapper { - std::map m_; - public: - void add(std::vector &l) { - int id = 0; - for (std::map::const_iterator it = m_.begin(); it != m_.end(); ++it) { - if (id < it->second) id = it->second + 1; - } - for (size_t i = 0; i < l.size(); ++i) { - if (m_.find(l[i]) == m_.end()) { - m_[l[i]] = id; - id++; - } - } +public: + void add(std::vector& l) + { + int id = 0; + for (std::map::const_iterator it = m_.begin(); it != m_.end(); ++it) + { + if (id < it->second) + id = it->second + 1; } - std::vector get(std::vector &v) { - std::vector ret(v.size()); - for (size_t i = 0; i < v.size(); ++i) { - ret[i] = m_[v[i]]; + for (size_t i = 0; i < l.size(); ++i) + { + if (m_.find(l[i]) == m_.end()) + { + m_[l[i]] = id; + id++; } - return ret; } - std::string lookup(int id) { - for (std::map::const_iterator it = m_.begin(); it != m_.end(); ++it) { - if (it->second == id) return it->first; - } - return "nan"; + } + std::vector get(std::vector& v) + { + std::vector ret(v.size()); + for (size_t i = 0; i < v.size(); ++i) + { + ret[i] = m_[v[i]]; } - const std::map& getMap() const { return m_; } + return ret; + } + std::string lookup(int id) + { + for (std::map::const_iterator it = m_.begin(); it != m_.end(); ++it) + { + if (it->second == id) + return it->first; + } + return "nan"; + } + const std::map& getMap() const + { + return m_; + } - void debugPrint() { - ROS_WARN_STREAM("label mapper: debug print:"); - for (std::map::const_iterator it = m_.begin(); it != m_.end(); ++it) { - ROS_WARN_STREAM("\t" << it->first << ": " << it->second); - } - ROS_WARN_STREAM("label mapper: debug print end"); + void debugPrint() + { + ROS_WARN_STREAM("label mapper: debug print:"); + for (std::map::const_iterator it = m_.begin(); it != m_.end(); ++it) + { + ROS_WARN_STREAM("\t" << it->first << ": " << it->second); } - }; + ROS_WARN_STREAM("label mapper: debug print end"); + } +}; - class Storage { - fs::path base_dir_; - public: - Storage(const fs::path &base_dir) { - base_dir_ = fs::user_expanded_path(base_dir); - if (!fs::exists(base_dir_)) { - init(); - } - if (!fs::is_directory(base_dir_)) { - std::stringstream ss; - ss << base_dir_ << " is not a directory"; - throw std::runtime_error(ss.str()); - } - }; - void init() { - if (!fs::create_directories(base_dir_)) { - std::stringstream ss; - ss << "failed to initialize directory: " << base_dir_; - throw std::runtime_error(ss.str()); - } +class Storage +{ + fs::path base_dir_; + +public: + Storage(const fs::path& base_dir) + { + base_dir_ = fs::user_expanded_path(base_dir); + if (!fs::exists(base_dir_)) + { + init(); + } + if (!fs::is_directory(base_dir_)) + { + std::stringstream ss; + ss << base_dir_ << " is not a directory"; + throw std::runtime_error(ss.str()); + } + }; + void init() + { + if (!fs::create_directories(base_dir_)) + { + std::stringstream ss; + ss << "failed to initialize directory: " << base_dir_; + throw std::runtime_error(ss.str()); } + } - void load(std::vector &images, std::vector &labels, bool append = true) { - if (!append) { - images.clear(); - labels.clear(); - } - fs::directory_iterator end; - for (fs::directory_iterator it(base_dir_); it != end; ++it) { - if (fs::is_directory(*it)) { - std::string label = it->path().stem().string(); - for (fs::directory_iterator cit(it->path()); cit != end; ++cit) { - if (fs::is_directory(*cit)) continue; - fs::path file_path = cit->path(); - try { - cv::Mat img = cv::imread(file_path.string(), CV_LOAD_IMAGE_COLOR); - labels.push_back(label); - images.push_back(img); - } catch (cv::Exception &e) { - ROS_ERROR_STREAM("Error: load image from " << file_path << ": " << e.what()); - } + void load(std::vector& images, std::vector& labels, bool append = true) + { + if (!append) + { + images.clear(); + labels.clear(); + } + fs::directory_iterator end; + for (fs::directory_iterator it(base_dir_); it != end; ++it) + { + if (fs::is_directory(*it)) + { + std::string label = it->path().stem().string(); + for (fs::directory_iterator cit(it->path()); cit != end; ++cit) + { + if (fs::is_directory(*cit)) + continue; + fs::path file_path = cit->path(); + try + { + cv::Mat img = cv::imread(file_path.string(), CV_LOAD_IMAGE_COLOR); + labels.push_back(label); + images.push_back(img); + } + catch (cv::Exception& e) + { + ROS_ERROR_STREAM("Error: load image from " << file_path << ": " << e.what()); } } } } + } - void save(const std::vector &images, const std::vector &labels) { - if (images.size() != labels.size()) { - throw std::length_error("images.size() != labels.size()"); - } - for (size_t i = 0; i < images.size(); ++i) { - save(images[i], labels[i]); - } + void save(const std::vector& images, const std::vector& labels) + { + if (images.size() != labels.size()) + { + throw std::length_error("images.size() != labels.size()"); } - - void save(const cv::Mat &image, const std::string &label) { - fs::path img_dir = base_dir_ / fs::path(label); - if (!fs::exists(img_dir) && !fs::create_directories(img_dir)) { - std::stringstream ss; - ss << "failed to initialize directory: " << img_dir; - throw std::runtime_error(ss.str()); - } - fs::directory_iterator end; - int file_count = 0; - for (fs::directory_iterator it(img_dir); it != end; ++it) { - if (!fs::is_directory(*it)) file_count++; - } - - std::stringstream ss; - // data_dir/person_label/person_label_123456.jpg - ss << label << "_" << std::setw(6) << std::setfill('0') << file_count << ".jpg"; - fs::path file_path = img_dir / ss.str(); - ROS_INFO_STREAM("saving image to :" << file_path); - try { - cv::imwrite(file_path.string(), image); - } catch(cv::Exception &e) { - ROS_ERROR_STREAM("Error: save image to " << file_path << ": " << e.what()); - } + for (size_t i = 0; i < images.size(); ++i) + { + save(images[i], labels[i]); } - }; + } - class FaceRecognitionNodelet: public opencv_apps::Nodelet + void save(const cv::Mat& image, const std::string& label) { - typedef opencv_apps::FaceRecognitionConfig Config; - typedef dynamic_reconfigure::Server Server; - typedef message_filters::sync_policies::ExactTime< - sensor_msgs::Image, opencv_apps::FaceArrayStamped> SyncPolicy; - typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::Image, opencv_apps::FaceArrayStamped> ApproximateSyncPolicy; - - Config config_; - boost::shared_ptr cfg_srv_; - boost::shared_ptr it_; - boost::shared_ptr > sync_; - boost::shared_ptr > async_; - image_transport::SubscriberFilter img_sub_; - message_filters::Subscriber face_sub_; - ros::Publisher debug_img_pub_; - ros::Publisher face_pub_; - ros::ServiceServer train_srv_; - - bool save_train_data_; - bool use_async_; - bool use_saved_data_; - double face_padding_; - int queue_size_; - std::string data_dir_; - boost::mutex mutex_; - - boost::shared_ptr label_mapper_; - boost::shared_ptr storage_; - cv::Size face_model_size_; - cv::Ptr model_; - - void drawFace(cv::Mat &img, const opencv_apps::Face &face) { - cv::Rect r(int(face.face.x - face.face.width / 2.0 - face.face.width * face_padding_ / 2.0), - int(face.face.y - face.face.height / 2.0 - face.face.height * face_padding_ / 2.0), - int(face.face.width + face.face.width * face_padding_), - int(face.face.height + face.face.height * face_padding_)); - cv::Scalar color(0.0, 0.0, 255.0); - int boldness = 2; - cv::rectangle(img, r.tl(), r.br(), color, boldness, CV_AA); - - double font_scale = 1.5; - int text_height = 20; - cv::Point text_bl; - if (r.br().y + text_height > img.rows) text_bl = r.tl() + cv::Point(0, -text_height); - else text_bl = r.br() + cv::Point(-r.width, text_height); + fs::path img_dir = base_dir_ / fs::path(label); + if (!fs::exists(img_dir) && !fs::create_directories(img_dir)) + { std::stringstream ss; - ss << face.label << " (" << std::fixed << std::setprecision(2) << face.confidence << ")"; - cv::putText(img, ss.str(), text_bl, cv::FONT_HERSHEY_PLAIN, font_scale, color, boldness, CV_AA); + ss << "failed to initialize directory: " << img_dir; + throw std::runtime_error(ss.str()); } - - void extractImage(const cv::Mat &img, const opencv_apps::Rect &rect, cv::Mat &ret, double padding = 0.0) { - int x = std::max(0, int(rect.x - rect.width / 2.0 - rect.width * padding / 2.0)); - int y = std::max(0, int(rect.y - rect.height / 2.0 - rect.height * padding / 2.0)); - cv::Rect r(x, y, - std::min(img.cols - x, int(rect.width + rect.width * padding)), - std::min(img.rows - y, int(rect.height + rect.height * padding))); - ret = img(r); + fs::directory_iterator end; + int file_count = 0; + for (fs::directory_iterator it(img_dir); it != end; ++it) + { + if (!fs::is_directory(*it)) + file_count++; } - void extractImage(const cv::Mat &img, const opencv_apps::Face &face, cv::Mat &ret, double padding = 0.0) { - extractImage(img, face.face, ret, padding); + std::stringstream ss; + // data_dir/person_label/person_label_123456.jpg + ss << label << "_" << std::setw(6) << std::setfill('0') << file_count << ".jpg"; + fs::path file_path = img_dir / ss.str(); + ROS_INFO_STREAM("saving image to :" << file_path); + try + { + cv::imwrite(file_path.string(), image); } - - void retrain() { - NODELET_DEBUG("retrain"); - std::vector images; - std::vector labels; - train(images, labels); + catch (cv::Exception& e) + { + ROS_ERROR_STREAM("Error: save image to " << file_path << ": " << e.what()); } + } +}; - void train(std::vector &images, std::vector &labels) { - size_t new_image_size = images.size(); +class FaceRecognitionNodelet : public opencv_apps::Nodelet +{ + typedef opencv_apps::FaceRecognitionConfig Config; + typedef dynamic_reconfigure::Server Server; + typedef message_filters::sync_policies::ExactTime SyncPolicy; + typedef message_filters::sync_policies::ApproximateTime + ApproximateSyncPolicy; + + Config config_; + boost::shared_ptr cfg_srv_; + boost::shared_ptr it_; + boost::shared_ptr > sync_; + boost::shared_ptr > async_; + image_transport::SubscriberFilter img_sub_; + message_filters::Subscriber face_sub_; + ros::Publisher debug_img_pub_; + ros::Publisher face_pub_; + ros::ServiceServer train_srv_; + + bool save_train_data_; + bool use_async_; + bool use_saved_data_; + double face_padding_; + int queue_size_; + std::string data_dir_; + boost::mutex mutex_; + + boost::shared_ptr label_mapper_; + boost::shared_ptr storage_; + cv::Size face_model_size_; + cv::Ptr model_; + + void drawFace(cv::Mat& img, const opencv_apps::Face& face) + { + cv::Rect r(int(face.face.x - face.face.width / 2.0 - face.face.width * face_padding_ / 2.0), + int(face.face.y - face.face.height / 2.0 - face.face.height * face_padding_ / 2.0), + int(face.face.width + face.face.width * face_padding_), + int(face.face.height + face.face.height * face_padding_)); + cv::Scalar color(0.0, 0.0, 255.0); + int boldness = 2; + cv::rectangle(img, r.tl(), r.br(), color, boldness, CV_AA); + + double font_scale = 1.5; + int text_height = 20; + cv::Point text_bl; + if (r.br().y + text_height > img.rows) + text_bl = r.tl() + cv::Point(0, -text_height); + else + text_bl = r.br() + cv::Point(-r.width, text_height); + std::stringstream ss; + ss << face.label << " (" << std::fixed << std::setprecision(2) << face.confidence << ")"; + cv::putText(img, ss.str(), text_bl, cv::FONT_HERSHEY_PLAIN, font_scale, color, boldness, CV_AA); + } - if (use_saved_data_) { - storage_->load(images, labels); - } + void extractImage(const cv::Mat& img, const opencv_apps::Rect& rect, cv::Mat& ret, double padding = 0.0) + { + int x = std::max(0, int(rect.x - rect.width / 2.0 - rect.width * padding / 2.0)); + int y = std::max(0, int(rect.y - rect.height / 2.0 - rect.height * padding / 2.0)); + cv::Rect r(x, y, std::min(img.cols - x, int(rect.width + rect.width * padding)), + std::min(img.rows - y, int(rect.height + rect.height * padding))); + ret = img(r); + } - if (images.size() == 0) return; + void extractImage(const cv::Mat& img, const opencv_apps::Face& face, cv::Mat& ret, double padding = 0.0) + { + extractImage(img, face.face, ret, padding); + } - std::vector resized_images(images.size()); - for (int i = 0; i < images.size(); ++i) { - cv::resize(images[i], resized_images[i], face_model_size_, - 0, 0, cv::INTER_CUBIC); - cv::cvtColor(resized_images[i], resized_images[i], CV_BGR2GRAY); - } + void retrain() + { + NODELET_DEBUG("retrain"); + std::vector images; + std::vector labels; + train(images, labels); + } - label_mapper_->add(labels); - std::vector ids = label_mapper_->get(labels); - NODELET_INFO_STREAM("training with " << images.size() << " images"); - // label_mapper_->debugPrint(); - model_->train(resized_images, ids); + void train(std::vector& images, std::vector& labels) + { + size_t new_image_size = images.size(); - if (save_train_data_ && new_image_size > 0) { - std::vector new_images(images.begin(), images.begin()+new_image_size); - std::vector new_labels(labels.begin(), labels.begin()+new_image_size); - storage_->save(new_images, new_labels); - } + if (use_saved_data_) + { + storage_->load(images, labels); + } + + if (images.size() == 0) + return; + + std::vector resized_images(images.size()); + for (int i = 0; i < images.size(); ++i) + { + cv::resize(images[i], resized_images[i], face_model_size_, 0, 0, cv::INTER_CUBIC); + cv::cvtColor(resized_images[i], resized_images[i], CV_BGR2GRAY); } - void predict(const cv::Mat &img, int &label, double &confidence) { - cv::Mat resized_img; - cv::resize(img, resized_img, face_model_size_, 0, 0, cv::INTER_CUBIC); - cv::cvtColor(resized_img, resized_img, CV_BGR2GRAY); - model_->predict(resized_img, label, confidence); + label_mapper_->add(labels); + std::vector ids = label_mapper_->get(labels); + NODELET_INFO_STREAM("training with " << images.size() << " images"); + // label_mapper_->debugPrint(); + model_->train(resized_images, ids); + + if (save_train_data_ && new_image_size > 0) + { + std::vector new_images(images.begin(), images.begin() + new_image_size); + std::vector new_labels(labels.begin(), labels.begin() + new_image_size); + storage_->save(new_images, new_labels); } + } - void faceImageCallback(const sensor_msgs::Image::ConstPtr &image, - const opencv_apps::FaceArrayStamped::ConstPtr &faces) { - NODELET_DEBUG("faceImageCallback"); - boost::mutex::scoped_lock lock(mutex_); + void predict(const cv::Mat& img, int& label, double& confidence) + { + cv::Mat resized_img; + cv::resize(img, resized_img, face_model_size_, 0, 0, cv::INTER_CUBIC); + cv::cvtColor(resized_img, resized_img, CV_BGR2GRAY); + model_->predict(resized_img, label, confidence); + } - // check if the face data is being trained - if(label_mapper_->getMap().empty()) - { - NODELET_WARN_THROTTLE(1.0, "Face data is not trained. Please train first."); - return; - } + void faceImageCallback(const sensor_msgs::Image::ConstPtr& image, + const opencv_apps::FaceArrayStamped::ConstPtr& faces) + { + NODELET_DEBUG("faceImageCallback"); + boost::mutex::scoped_lock lock(mutex_); + + // check if the face data is being trained + if (label_mapper_->getMap().empty()) + { + NODELET_WARN_THROTTLE(1.0, "Face data is not trained. Please train first."); + return; + } - // check if need to draw and publish debug image - bool publish_debug_image = debug_img_pub_.getNumSubscribers() > 0; - - try { - cv::Mat cv_img = cv_bridge::toCvShare(image, enc::BGR8)->image; - opencv_apps::FaceArrayStamped ret_msg = *faces; - for (size_t i = 0; i < faces->faces.size(); ++i) { - cv::Mat face_img, resized_image; - extractImage(cv_img, faces->faces[i], face_img, face_padding_); - - int label_id = -1; - double confidence = 0.0; - predict(face_img, label_id, confidence); - if (label_id == -1) continue; - ret_msg.faces[i].label = label_mapper_->lookup(label_id); - ret_msg.faces[i].confidence = confidence; - - // draw debug image - if (publish_debug_image) drawFace(cv_img, ret_msg.faces[i]); - } - face_pub_.publish(ret_msg); - if (publish_debug_image) { - sensor_msgs::Image::Ptr debug_img - = cv_bridge::CvImage(image->header, enc::BGR8, cv_img).toImageMsg(); - debug_img_pub_.publish(debug_img); - NODELET_DEBUG("Published debug image"); - } - } catch (cv::Exception &e) { - NODELET_ERROR_STREAM("error at image processing: " - << e.err << " " - << e.func << " " - << e.file << " " - << e.line); + // check if need to draw and publish debug image + bool publish_debug_image = debug_img_pub_.getNumSubscribers() > 0; + + try + { + cv::Mat cv_img = cv_bridge::toCvShare(image, enc::BGR8)->image; + opencv_apps::FaceArrayStamped ret_msg = *faces; + for (size_t i = 0; i < faces->faces.size(); ++i) + { + cv::Mat face_img, resized_image; + extractImage(cv_img, faces->faces[i], face_img, face_padding_); + + int label_id = -1; + double confidence = 0.0; + predict(face_img, label_id, confidence); + if (label_id == -1) + continue; + ret_msg.faces[i].label = label_mapper_->lookup(label_id); + ret_msg.faces[i].confidence = confidence; + + // draw debug image + if (publish_debug_image) + drawFace(cv_img, ret_msg.faces[i]); + } + face_pub_.publish(ret_msg); + if (publish_debug_image) + { + sensor_msgs::Image::Ptr debug_img = cv_bridge::CvImage(image->header, enc::BGR8, cv_img).toImageMsg(); + debug_img_pub_.publish(debug_img); + NODELET_DEBUG("Published debug image"); } } + catch (cv::Exception& e) + { + NODELET_ERROR_STREAM("error at image processing: " << e.err << " " << e.func << " " << e.file << " " << e.line); + } + } - bool trainCallback(opencv_apps::FaceRecognitionTrain::Request &req, - opencv_apps::FaceRecognitionTrain::Response &res) { - boost::mutex::scoped_lock lock(mutex_); - try { - std::vector images(req.images.size()); - bool use_roi = req.rects.size() == 0 ? false : true; - - if (use_roi && req.images.size() != req.rects.size()) { - throw std::length_error("req.images.size() != req.rects.size()"); - } + bool trainCallback(opencv_apps::FaceRecognitionTrain::Request& req, opencv_apps::FaceRecognitionTrain::Response& res) + { + boost::mutex::scoped_lock lock(mutex_); + try + { + std::vector images(req.images.size()); + bool use_roi = req.rects.size() == 0 ? false : true; + + if (use_roi && req.images.size() != req.rects.size()) + { + throw std::length_error("req.images.size() != req.rects.size()"); + } - for (size_t i = 0; i < req.images.size(); ++i) { - sensor_msgs::Image img = req.images[i]; - images[i] = cv_bridge::toCvCopy(img, enc::BGR8)->image; - if (use_roi) { - cv::Mat face_img; - extractImage(images[i], req.rects[i], face_img); - images[i] = face_img; - } + for (size_t i = 0; i < req.images.size(); ++i) + { + sensor_msgs::Image img = req.images[i]; + images[i] = cv_bridge::toCvCopy(img, enc::BGR8)->image; + if (use_roi) + { + cv::Mat face_img; + extractImage(images[i], req.rects[i], face_img); + images[i] = face_img; } - std::vector labels(req.labels.begin(), req.labels.end()); - train(images, labels); - res.ok = true; - return true; - } catch (cv::Exception &e) { - std::stringstream ss; - ss << "error at training: " - << e.err << " " - << e.func << " " - << e.file << " " - << e.line; - res.ok = false; - res.error = ss.str(); } - return false; + std::vector labels(req.labels.begin(), req.labels.end()); + train(images, labels); + res.ok = true; + return true; } + catch (cv::Exception& e) + { + std::stringstream ss; + ss << "error at training: " << e.err << " " << e.func << " " << e.file << " " << e.line; + res.ok = false; + res.error = ss.str(); + } + return false; + } - void configCallback(Config &config, uint32_t level) { - boost::mutex::scoped_lock lock(mutex_); + void configCallback(Config& config, uint32_t level) + { + boost::mutex::scoped_lock lock(mutex_); - bool need_recreate_model = false; - bool need_retrain = false; + bool need_recreate_model = false; + bool need_retrain = false; - use_saved_data_ = config.use_saved_data; - save_train_data_ = config.save_train_data; - face_padding_ = config.face_padding; + use_saved_data_ = config.use_saved_data; + save_train_data_ = config.save_train_data; + face_padding_ = config.face_padding; - if (face_model_size_.width != config.face_model_width) { - face_model_size_.width = config.face_model_width; - need_retrain = true; - } - if (face_model_size_.height != config.face_model_height) { - face_model_size_.height = config.face_model_height; - need_retrain = true; - } + if (face_model_size_.width != config.face_model_width) + { + face_model_size_.width = config.face_model_width; + need_retrain = true; + } + if (face_model_size_.height != config.face_model_height) + { + face_model_size_.height = config.face_model_height; + need_retrain = true; + } - if (data_dir_ != config.data_dir) { - data_dir_ = config.data_dir; - need_retrain = true; - label_mapper_.reset(new LabelMapper()); - storage_.reset(new Storage(fs::path(data_dir_))); - } + if (data_dir_ != config.data_dir) + { + data_dir_ = config.data_dir; + need_retrain = true; + label_mapper_.reset(new LabelMapper()); + storage_.reset(new Storage(fs::path(data_dir_))); + } - if (config_.model_method != config.model_method) { - need_recreate_model = true; - } + if (config_.model_method != config.model_method) + { + need_recreate_model = true; + } - if (config_.model_num_components != config.model_num_components) { - need_recreate_model = true; - } + if (config_.model_num_components != config.model_num_components) + { + need_recreate_model = true; + } - if (config.model_method == "LBPH" && ( - config_.lbph_radius != config.lbph_radius || - config_.lbph_neighbors != config.lbph_neighbors || - config_.lbph_grid_x != config.lbph_grid_x || - config_.lbph_grid_y != config.lbph_grid_y )) { - need_recreate_model = true; - } + if (config.model_method == "LBPH" && + (config_.lbph_radius != config.lbph_radius || config_.lbph_neighbors != config.lbph_neighbors || + config_.lbph_grid_x != config.lbph_grid_x || config_.lbph_grid_y != config.lbph_grid_y)) + { + need_recreate_model = true; + } - if (need_recreate_model) { - try { - if (config.model_method == "eigen") { + if (need_recreate_model) + { + try + { + if (config.model_method == "eigen") + { // https://docs.opencv.org/3.3.1/da/d60/tutorial_face_main.html #if CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION >= 3 - model_ = face::EigenFaceRecognizer::create(config.model_num_components, - config.model_threshold); + model_ = face::EigenFaceRecognizer::create(config.model_num_components, config.model_threshold); #else - model_ = face::createEigenFaceRecognizer(config.model_num_components, - config.model_threshold); + model_ = face::createEigenFaceRecognizer(config.model_num_components, config.model_threshold); #endif - } else if (config.model_method == "fisher") { + } + else if (config.model_method == "fisher") + { #if CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION >= 3 - model_ = face::FisherFaceRecognizer::create(config.model_num_components, - config.model_threshold); + model_ = face::FisherFaceRecognizer::create(config.model_num_components, config.model_threshold); #else - model_ = face::createFisherFaceRecognizer(config.model_num_components, - config.model_threshold); + model_ = face::createFisherFaceRecognizer(config.model_num_components, config.model_threshold); #endif - } else if (config.model_method == "LBPH") { + } + else if (config.model_method == "LBPH") + { #if CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION >= 3 - model_ = face::LBPHFaceRecognizer::create(config.lbph_radius, - config.lbph_neighbors, - config.lbph_grid_x, - config.lbph_grid_y); + model_ = face::LBPHFaceRecognizer::create(config.lbph_radius, config.lbph_neighbors, config.lbph_grid_x, + config.lbph_grid_y); #else - model_ = face::createLBPHFaceRecognizer(config.lbph_radius, - config.lbph_neighbors, - config.lbph_grid_x, + model_ = face::createLBPHFaceRecognizer(config.lbph_radius, config.lbph_neighbors, config.lbph_grid_x, config.lbph_grid_y); #endif - } - need_retrain = true; - } catch (cv::Exception &e) { - NODELET_ERROR_STREAM("Error: create face recognizer: " << e.what()); } + need_retrain = true; + } + catch (cv::Exception& e) + { + NODELET_ERROR_STREAM("Error: create face recognizer: " << e.what()); } + } - if (need_retrain) { - try { - retrain(); - } catch (cv::Exception &e) { - NODELET_ERROR_STREAM("Error: train: " << e.what()); - } + if (need_retrain) + { + try + { + retrain(); + } + catch (cv::Exception& e) + { + NODELET_ERROR_STREAM("Error: train: " << e.what()); } + } - if (config_.model_threshold != config.model_threshold) { - try { + if (config_.model_threshold != config.model_threshold) + { + try + { #if CV_MAJOR_VERSION >= 3 - if(face::BasicFaceRecognizer* model = dynamic_cast(model_.get())) { - model->setThreshold(config.model_threshold); - } else if (face::LBPHFaceRecognizer* model = dynamic_cast(model_.get())) { - model->setThreshold(config.model_threshold); - } + if (face::BasicFaceRecognizer* model = dynamic_cast(model_.get())) + { + model->setThreshold(config.model_threshold); + } + else if (face::LBPHFaceRecognizer* model = dynamic_cast(model_.get())) + { + model->setThreshold(config.model_threshold); + } #else - model_->set("threshold", config.model_threshold); + model_->set("threshold", config.model_threshold); #endif - } catch (cv::Exception &e) { - NODELET_ERROR_STREAM("Error: set threshold: " << e.what()); - } } - config_ = config; - } - - void subscribe() { - NODELET_DEBUG("subscribe"); - img_sub_.subscribe(*it_, "image", 1); - face_sub_.subscribe(*nh_, "faces", 1); - if (use_async_) { - async_ = boost::make_shared >(queue_size_); - async_->connectInput(img_sub_, face_sub_); - async_->registerCallback(boost::bind(&FaceRecognitionNodelet::faceImageCallback, this, _1, _2)); - } else { - sync_ = boost::make_shared >(queue_size_); - sync_->connectInput(img_sub_, face_sub_); - sync_->registerCallback(boost::bind(&FaceRecognitionNodelet::faceImageCallback, this, _1, _2)); + catch (cv::Exception& e) + { + NODELET_ERROR_STREAM("Error: set threshold: " << e.what()); } } + config_ = config; + } - void unsubscribe() { - NODELET_DEBUG("unsubscribe"); - img_sub_.unsubscribe(); - face_sub_.unsubscribe(); + void subscribe() + { + NODELET_DEBUG("subscribe"); + img_sub_.subscribe(*it_, "image", 1); + face_sub_.subscribe(*nh_, "faces", 1); + if (use_async_) + { + async_ = boost::make_shared >(queue_size_); + async_->connectInput(img_sub_, face_sub_); + async_->registerCallback(boost::bind(&FaceRecognitionNodelet::faceImageCallback, this, _1, _2)); } + else + { + sync_ = boost::make_shared >(queue_size_); + sync_->connectInput(img_sub_, face_sub_); + sync_->registerCallback(boost::bind(&FaceRecognitionNodelet::faceImageCallback, this, _1, _2)); + } + } + + void unsubscribe() + { + NODELET_DEBUG("unsubscribe"); + img_sub_.unsubscribe(); + face_sub_.unsubscribe(); + } - public: - virtual void onInit() { - Nodelet::onInit(); +public: + virtual void onInit() + { + Nodelet::onInit(); - // variables - face_model_size_ = cv::Size(190, 90); + // variables + face_model_size_ = cv::Size(190, 90); - // dynamic reconfigures - cfg_srv_ = boost::make_shared(*pnh_); - Server::CallbackType f = boost::bind(&FaceRecognitionNodelet::configCallback, this, _1, _2); - cfg_srv_->setCallback(f); + // dynamic reconfigures + cfg_srv_ = boost::make_shared(*pnh_); + Server::CallbackType f = boost::bind(&FaceRecognitionNodelet::configCallback, this, _1, _2); + cfg_srv_->setCallback(f); - // parameters - pnh_->param("approximate_sync", use_async_, false); - pnh_->param("queue_size", queue_size_, 100); + // parameters + pnh_->param("approximate_sync", use_async_, false); + pnh_->param("queue_size", queue_size_, 100); - // advertise - debug_img_pub_ = advertise(*pnh_, "debug_image", 1); - face_pub_ = advertise(*pnh_, "output", 1); - train_srv_ = pnh_->advertiseService("train", - &FaceRecognitionNodelet::trainCallback, this); - it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + // advertise + debug_img_pub_ = advertise(*pnh_, "debug_image", 1); + face_pub_ = advertise(*pnh_, "output", 1); + train_srv_ = pnh_->advertiseService("train", &FaceRecognitionNodelet::trainCallback, this); + it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); - onInitPostProcess(); - } - }; -} // namespace opencv_apps + onInitPostProcess(); + } +}; +} // namespace opencv_apps -namespace face_recognition { -class FaceRecognitionNodelet : public opencv_apps::FaceRecognitionNodelet { +namespace face_recognition +{ +class FaceRecognitionNodelet : public opencv_apps::FaceRecognitionNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet face_recognition/face_recognition is deprecated, " "and renamed to opencv_apps/face_recognition."); opencv_apps::FaceRecognitionNodelet::onInit(); } }; -} // namespace face_recognition - +} // namespace face_recognition #include PLUGINLIB_EXPORT_CLASS(opencv_apps::FaceRecognitionNodelet, nodelet::Nodelet); diff --git a/src/nodelet/fback_flow_nodelet.cpp b/src/nodelet/fback_flow_nodelet.cpp index 720059c5..604e1b32 100644 --- a/src/nodelet/fback_flow_nodelet.cpp +++ b/src/nodelet/fback_flow_nodelet.cpp @@ -52,7 +52,8 @@ #include "opencv_apps/FBackFlowConfig.h" #include "opencv_apps/FlowArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class FBackFlowNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -76,12 +77,12 @@ class FBackFlowNodelet : public opencv_apps::Nodelet cv::Mat prevgray, gray, flow, cflow; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -98,7 +99,7 @@ class FBackFlowNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -115,69 +116,74 @@ class FBackFlowNodelet : public opencv_apps::Nodelet opencv_apps::FlowArrayStamped flows_msg; flows_msg.header = msg->header; - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); - if (need_config_update_) { + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + if (need_config_update_) + { reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } // Check if clock is jumped back - if (ros::Time::isSimTime() && prev_stamp_ > msg->header.stamp) { + if (ros::Time::isSimTime() && prev_stamp_ > msg->header.stamp) + { NODELET_WARN_STREAM("Detected jump back in time of " << msg->header.stamp << ". Clearing optical flow cache."); prevgray = cv::Mat(); } // Do the work - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, gray, cv::COLOR_BGR2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); + } + else + { frame.copyTo(gray); } - if( prevgray.data ) + if (prevgray.data) { cv::calcOpticalFlowFarneback(prevgray, gray, flow, 0.5, 3, 15, 3, 5, 1.2, 0); cv::cvtColor(prevgray, cflow, cv::COLOR_GRAY2BGR); - //drawOptFlowMap(flow, cflow, 16, 1.5, Scalar(0, 255, 0)); + // drawOptFlowMap(flow, cflow, 16, 1.5, Scalar(0, 255, 0)); int step = 16; cv::Scalar color = cv::Scalar(0, 255, 0); - for(int y = 0; y < cflow.rows; y += step) - for(int x = 0; x < cflow.cols; x += step) - { - const cv::Point2f& fxy = flow.at(y, x); - cv::line(cflow, cv::Point(x,y), cv::Point(cvRound(x+fxy.x), cvRound(y+fxy.y)), - color); - cv::circle(cflow, cv::Point(x,y), 2, color, -1); - - opencv_apps::Flow flow_msg; - opencv_apps::Point2D point_msg; - opencv_apps::Point2D velocity_msg; - point_msg.x = x; - point_msg.y = y; - velocity_msg.x = fxy.x; - velocity_msg.y = fxy.y; - flow_msg.point = point_msg; - flow_msg.velocity = velocity_msg; - flows_msg.flow.push_back(flow_msg); - } + for (int y = 0; y < cflow.rows; y += step) + for (int x = 0; x < cflow.cols; x += step) + { + const cv::Point2f& fxy = flow.at(y, x); + cv::line(cflow, cv::Point(x, y), cv::Point(cvRound(x + fxy.x), cvRound(y + fxy.y)), color); + cv::circle(cflow, cv::Point(x, y), 2, color, -1); + + opencv_apps::Flow flow_msg; + opencv_apps::Point2D point_msg; + opencv_apps::Point2D velocity_msg; + point_msg.x = x; + point_msg.y = y; + velocity_msg.x = fxy.x; + velocity_msg.y = fxy.y; + flow_msg.point = point_msg; + flow_msg.velocity = velocity_msg; + flows_msg.flow.push_back(flow_msg); + } } std::swap(prevgray, gray); //-- Show what you got - if( debug_view_) { - cv::imshow( window_name_, cflow ); + if (debug_view_) + { + cv::imshow(window_name_, cflow); int c = cv::waitKey(1); } - // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", cflow).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(flows_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -209,16 +215,17 @@ class FBackFlowNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "flow"; - + reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&FBackFlowNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&FBackFlowNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -228,19 +235,21 @@ class FBackFlowNodelet : public opencv_apps::Nodelet } }; bool FBackFlowNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace fback_flow { -class FBackFlowNodelet : public opencv_apps::FBackFlowNodelet { +namespace fback_flow +{ +class FBackFlowNodelet : public opencv_apps::FBackFlowNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet fback_flow/fback_flow is deprecated, " "and renamed to opencv_apps/fback_flow."); opencv_apps::FBackFlowNodelet::onInit(); } }; -} // namespace fback_flow - +} // namespace fback_flow #include PLUGINLIB_EXPORT_CLASS(opencv_apps::FBackFlowNodelet, nodelet::Nodelet); diff --git a/src/nodelet/find_contours_nodelet.cpp b/src/nodelet/find_contours_nodelet.cpp index 52f0ba4b..732c6f33 100644 --- a/src/nodelet/find_contours_nodelet.cpp +++ b/src/nodelet/find_contours_nodelet.cpp @@ -3,11 +3,11 @@ * * Copyright (c) 2014, Kei Okada. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -55,7 +55,8 @@ #include "opencv_apps/ContourArray.h" #include "opencv_apps/ContourArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class FindContoursNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -79,13 +80,13 @@ class FindContoursNodelet : public opencv_apps::Nodelet std::string window_name_; static bool need_config_update_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; low_threshold_ = config_.canny_low_threshold; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -96,13 +97,13 @@ class FindContoursNodelet : public opencv_apps::Nodelet { do_work(msg, cam_info->header.frame_id); } - + void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -123,18 +124,22 @@ class FindContoursNodelet : public opencv_apps::Nodelet cv::Mat src_gray; /// Convert it to gray - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, src_gray, cv::COLOR_RGB2GRAY); + } + else + { src_gray = frame; } - cv::GaussianBlur( src_gray, src_gray, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT ); + cv::GaussianBlur(src_gray, src_gray, cv::Size(3, 3), 0, 0, cv::BORDER_DEFAULT); /// Create window - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); } - + cv::Mat canny_output; int max_thresh = 255; std::vector > contours; @@ -142,23 +147,24 @@ class FindContoursNodelet : public opencv_apps::Nodelet cv::RNG rng(12345); /// Reduce noise with a kernel 3x3 - cv::blur( src_gray, canny_output, cv::Size(3,3) ); + cv::blur(src_gray, canny_output, cv::Size(3, 3)); /// Canny detector - cv::Canny( canny_output, canny_output, low_threshold_, low_threshold_*2, 3 ); + cv::Canny(canny_output, canny_output, low_threshold_, low_threshold_ * 2, 3); /// Find contours - cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) ); + cv::findContours(canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0)); /// Draw contours - cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 ); - for( size_t i = 0; i< contours.size(); i++ ) + cv::Mat drawing = cv::Mat::zeros(canny_output.size(), CV_8UC3); + for (size_t i = 0; i < contours.size(); i++) { - cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ); - cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() ); - + cv::Scalar color = cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)); + cv::drawContours(drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point()); + opencv_apps::Contour contour_msg; - for ( size_t j = 0; j < contours[i].size(); j++ ) { + for (size_t j = 0; j < contours[i].size(); j++) + { opencv_apps::Point2D point_msg; point_msg.x = contours[i][j].x; point_msg.y = contours[i][j].y; @@ -168,24 +174,27 @@ class FindContoursNodelet : public opencv_apps::Nodelet } /// Create a Trackbar for user to enter threshold - if( debug_view_) { - if (need_config_update_) { + if (debug_view_) + { + if (need_config_update_) + { config_.canny_low_threshold = low_threshold_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } - cv::createTrackbar( "Canny thresh:", window_name_, &low_threshold_, max_thresh, trackbarCallback); + cv::createTrackbar("Canny thresh:", window_name_, &low_threshold_, max_thresh, trackbarCallback); - cv::imshow( window_name_, drawing ); + cv::imshow(window_name_, drawing); int c = cv::waitKey(1); } // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg(); + sensor_msgs::Image::Ptr out_img = + cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(contours_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -217,17 +226,18 @@ class FindContoursNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "Demo code to find contours in an image"; - low_threshold_ = 100; // only for canny - + low_threshold_ = 100; // only for canny + reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&FindContoursNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&FindContoursNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -237,19 +247,21 @@ class FindContoursNodelet : public opencv_apps::Nodelet } }; bool FindContoursNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace find_contours { -class FindContoursNodelet : public opencv_apps::FindContoursNodelet { +namespace find_contours +{ +class FindContoursNodelet : public opencv_apps::FindContoursNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet find_contours/find_contours is deprecated, " "and renamed to opencv_apps/find_contours."); opencv_apps::FindContoursNodelet::onInit(); } }; -} // namespace find_contours - +} // namespace find_contours #include PLUGINLIB_EXPORT_CLASS(opencv_apps::FindContoursNodelet, nodelet::Nodelet); diff --git a/src/nodelet/general_contours_nodelet.cpp b/src/nodelet/general_contours_nodelet.cpp index 2830bace..aaf18a6b 100644 --- a/src/nodelet/general_contours_nodelet.cpp +++ b/src/nodelet/general_contours_nodelet.cpp @@ -55,7 +55,8 @@ #include "opencv_apps/RotatedRectArray.h" #include "opencv_apps/RotatedRectArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class GeneralContoursNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -79,13 +80,13 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet std::string window_name_; static bool need_config_update_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; threshold_ = config_.threshold; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -102,7 +103,7 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -124,16 +125,20 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet cv::Mat src_gray; /// Convert image to gray and blur it - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, src_gray, cv::COLOR_RGB2GRAY); + } + else + { src_gray = frame; } - cv::blur( src_gray, src_gray, cv::Size(3,3) ); + cv::blur(src_gray, src_gray, cv::Size(3, 3)); /// Create window - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); } cv::Mat threshold_output; @@ -143,34 +148,38 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet cv::RNG rng(12345); /// Detect edges using Threshold - cv::threshold( src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY ); + cv::threshold(src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY); /// Find contours - cv::findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) ); + cv::findContours(threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0)); /// Find the rotated rectangles and ellipses for each contour - std::vector minRect( contours.size() ); - std::vector minEllipse( contours.size() ); + std::vector minRect(contours.size()); + std::vector minEllipse(contours.size()); - for( size_t i = 0; i < contours.size(); i++ ) - { minRect[i] = cv::minAreaRect( cv::Mat(contours[i]) ); - if( contours[i].size() > 5 ) - { minEllipse[i] = cv::fitEllipse( cv::Mat(contours[i]) ); } + for (size_t i = 0; i < contours.size(); i++) + { + minRect[i] = cv::minAreaRect(cv::Mat(contours[i])); + if (contours[i].size() > 5) + { + minEllipse[i] = cv::fitEllipse(cv::Mat(contours[i])); + } } /// Draw contours + rotated rects + ellipses - cv::Mat drawing = cv::Mat::zeros( threshold_output.size(), CV_8UC3 ); - for( size_t i = 0; i< contours.size(); i++ ) + cv::Mat drawing = cv::Mat::zeros(threshold_output.size(), CV_8UC3); + for (size_t i = 0; i < contours.size(); i++) { - cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ); + cv::Scalar color = cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)); // contour - cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector(), 0, cv::Point() ); + cv::drawContours(drawing, contours, (int)i, color, 1, 8, std::vector(), 0, cv::Point()); // ellipse - cv::ellipse( drawing, minEllipse[i], color, 2, 8 ); + cv::ellipse(drawing, minEllipse[i], color, 2, 8); // rotated rectangle - cv::Point2f rect_points[4]; minRect[i].points( rect_points ); - for( int j = 0; j < 4; j++ ) - cv::line( drawing, rect_points[j], rect_points[(j+1)%4], color, 1, 8 ); + cv::Point2f rect_points[4]; + minRect[i].points(rect_points); + for (int j = 0; j < 4; j++) + cv::line(drawing, rect_points[j], rect_points[(j + 1) % 4], color, 1, 8); opencv_apps::RotatedRect rect_msg; opencv_apps::Point2D rect_center; @@ -198,25 +207,28 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet } /// Create a Trackbar for user to enter threshold - if( debug_view_) { - if (need_config_update_) { + if (debug_view_) + { + if (need_config_update_) + { config_.threshold = threshold_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } - cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback); + cv::createTrackbar("Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback); - cv::imshow( window_name_, drawing ); + cv::imshow(window_name_, drawing); int c = cv::waitKey(1); } // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg(); + sensor_msgs::Image::Ptr out_img = + cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg(); img_pub_.publish(out_img); rects_pub_.publish(rects_msg); ellipses_pub_.publish(ellipses_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -248,7 +260,8 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -258,9 +271,9 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&GeneralContoursNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&GeneralContoursNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); - + img_pub_ = advertiseImage(*pnh_, "image", 1); rects_pub_ = advertise(*pnh_, "rectangles", 1); ellipses_pub_ = advertise(*pnh_, "ellipses", 1); @@ -269,19 +282,21 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet } }; bool GeneralContoursNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace general_contours { -class GeneralContoursNodelet : public opencv_apps::GeneralContoursNodelet { +namespace general_contours +{ +class GeneralContoursNodelet : public opencv_apps::GeneralContoursNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet general_contours/general_contours is deprecated, " "and renamed to opencv_apps/general_contours."); opencv_apps::GeneralContoursNodelet::onInit(); } }; -} // namespace general_contours - +} // namespace general_contours #include PLUGINLIB_EXPORT_CLASS(opencv_apps::GeneralContoursNodelet, nodelet::Nodelet); diff --git a/src/nodelet/goodfeature_track_nodelet.cpp b/src/nodelet/goodfeature_track_nodelet.cpp index 79a1aa43..406f9532 100644 --- a/src/nodelet/goodfeature_track_nodelet.cpp +++ b/src/nodelet/goodfeature_track_nodelet.cpp @@ -53,7 +53,8 @@ #include "opencv_apps/Point2D.h" #include "opencv_apps/Point2DArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -77,13 +78,13 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet int max_corners_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; max_corners_ = config_.max_corners; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -100,7 +101,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -121,18 +122,23 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet cv::Mat src_gray; int maxTrackbar = 100; - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, src_gray, cv::COLOR_BGR2GRAY); + } + else + { src_gray = frame; - cv::cvtColor( src_gray, frame, cv::COLOR_GRAY2BGR ); + cv::cvtColor(src_gray, frame, cv::COLOR_GRAY2BGR); } - if( debug_view_) { + if (debug_view_) + { /// Create Trackbars for Thresholds - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); - cv::createTrackbar( "Max corners", window_name_, &max_corners_, maxTrackbar, trackbarCallback); - if (need_config_update_) { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + cv::createTrackbar("Max corners", window_name_, &max_corners_, maxTrackbar, trackbarCallback); + if (need_config_update_) + { config_.max_corners = max_corners_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; @@ -140,7 +146,10 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet } /// void goodFeaturesToTrack_Demo( int, void* ) - if( max_corners_ < 1 ) { max_corners_ = 1; } + if (max_corners_ < 1) + { + max_corners_ = 1; + } /// Parameters for Shi-Tomasi algorithm std::vector corners; @@ -153,31 +162,28 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet cv::RNG rng(12345); /// Apply corner detection - cv::goodFeaturesToTrack( src_gray, - corners, - max_corners_, - qualityLevel, - minDistance, - cv::Mat(), - blockSize, - useHarrisDetector, - k ); - + cv::goodFeaturesToTrack(src_gray, corners, max_corners_, qualityLevel, minDistance, cv::Mat(), blockSize, + useHarrisDetector, k); /// Draw corners detected - NODELET_INFO_STREAM("** Number of corners detected: "<param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -230,9 +237,9 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&GoodfeatureTrackNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&GoodfeatureTrackNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); - + img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "corners", 1); @@ -240,19 +247,21 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet } }; bool GoodfeatureTrackNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace goodfeature_track { -class GoodfeatureTrackNodelet : public opencv_apps::GoodfeatureTrackNodelet { +namespace goodfeature_track +{ +class GoodfeatureTrackNodelet : public opencv_apps::GoodfeatureTrackNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet goodfeature_track/goodfeature_track is deprecated, " "and renamed to opencv_apps/goodfeature_track."); opencv_apps::GoodfeatureTrackNodelet::onInit(); } }; -} // namespace goodfeature_track - +} // namespace goodfeature_track #include PLUGINLIB_EXPORT_CLASS(opencv_apps::GoodfeatureTrackNodelet, nodelet::Nodelet); diff --git a/src/nodelet/hough_circles_nodelet.cpp b/src/nodelet/hough_circles_nodelet.cpp index 9b1eab67..687f606b 100644 --- a/src/nodelet/hough_circles_nodelet.cpp +++ b/src/nodelet/hough_circles_nodelet.cpp @@ -55,7 +55,8 @@ #include "opencv_apps/CircleArray.h" #include "opencv_apps/CircleArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class HoughCirclesNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -82,21 +83,27 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet int accumulator_threshold_initial_value_; int max_accumulator_threshold_; int max_canny_threshold_; - double canny_threshold_; int canny_threshold_int; // for trackbar - double accumulator_threshold_; int accumulator_threshold_int; + double canny_threshold_; + int canny_threshold_int; // for trackbar + double accumulator_threshold_; + int accumulator_threshold_int; int gaussian_blur_size_; - double gaussian_sigma_x_; int gaussian_sigma_x_int; - double gaussian_sigma_y_; int gaussian_sigma_y_int; + double gaussian_sigma_x_; + int gaussian_sigma_x_int; + double gaussian_sigma_y_; + int gaussian_sigma_y_int; int voting_threshold_; - double min_distance_between_circles_; int min_distance_between_circles_int; - double dp_; int dp_int; + double min_distance_between_circles_; + int min_distance_between_circles_int; + double dp_; + int dp_int; int min_circle_radius_; int max_circle_radius_; image_transport::Publisher debug_image_pub_; int debug_image_type_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; canny_threshold_ = config_.canny_threshold; @@ -118,7 +125,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet dp_int = int(dp_); } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -135,7 +142,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int value, void* userdata) + static void trackbarCallback(int value, void* userdata) { need_config_update_ = true; } @@ -156,33 +163,42 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet std::vector faces; cv::Mat src_gray, edges; - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, src_gray, cv::COLOR_BGR2GRAY); + } + else + { src_gray = frame; } // create the main window, and attach the trackbars - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); - cv::createTrackbar("Canny Threshold", window_name_, &canny_threshold_int, max_canny_threshold_, trackbarCallback); - cv::createTrackbar("Accumulator Threshold", window_name_, &accumulator_threshold_int, max_accumulator_threshold_, trackbarCallback); + cv::createTrackbar("Canny Threshold", window_name_, &canny_threshold_int, max_canny_threshold_, + trackbarCallback); + cv::createTrackbar("Accumulator Threshold", window_name_, &accumulator_threshold_int, + max_accumulator_threshold_, trackbarCallback); cv::createTrackbar("Gaussian Blur Size", window_name_, &gaussian_blur_size_, 30, trackbarCallback); cv::createTrackbar("Gaussian Sigam X", window_name_, &gaussian_sigma_x_int, 10, trackbarCallback); cv::createTrackbar("Gaussian Sigma Y", window_name_, &gaussian_sigma_y_int, 10, trackbarCallback); - cv::createTrackbar("Min Distance between Circles", window_name_, &min_distance_between_circles_int, 100, trackbarCallback); + cv::createTrackbar("Min Distance between Circles", window_name_, &min_distance_between_circles_int, 100, + trackbarCallback); cv::createTrackbar("Dp", window_name_, &dp_int, 100, trackbarCallback); cv::createTrackbar("Min Circle Radius", window_name_, &min_circle_radius_, 500, trackbarCallback); cv::createTrackbar("Max Circle Radius", window_name_, &max_circle_radius_, 2000, trackbarCallback); - if (need_config_update_) { + if (need_config_update_) + { config_.canny_threshold = canny_threshold_ = (double)canny_threshold_int; config_.accumulator_threshold = accumulator_threshold_ = (double)accumulator_threshold_int; config_.gaussian_blur_size = gaussian_blur_size_; config_.gaussian_sigma_x = gaussian_sigma_x_ = (double)gaussian_sigma_x_int; config_.gaussian_sigma_y = gaussian_sigma_y_ = (double)gaussian_sigma_y_int; - config_.min_distance_between_circles = min_distance_between_circles_ = (double)min_distance_between_circles_int; + config_.min_distance_between_circles = min_distance_between_circles_ = + (double)min_distance_between_circles_int; config_.dp = dp_ = (double)dp_int; config_.min_circle_radius = min_circle_radius_; config_.max_circle_radius = max_circle_radius_; @@ -193,54 +209,55 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet // Reduce the noise so we avoid false circle detection // gaussian_blur_size_ must be odd number - if (gaussian_blur_size_%2 != 1) { + if (gaussian_blur_size_ % 2 != 1) + { gaussian_blur_size_ = gaussian_blur_size_ + 1; } - cv::GaussianBlur( src_gray, src_gray, cv::Size(gaussian_blur_size_, gaussian_blur_size_), gaussian_sigma_x_, gaussian_sigma_y_ ); + cv::GaussianBlur(src_gray, src_gray, cv::Size(gaussian_blur_size_, gaussian_blur_size_), gaussian_sigma_x_, + gaussian_sigma_y_); // those paramaters cannot be =0 // so we must check here canny_threshold_ = std::max(canny_threshold_, 1.0); accumulator_threshold_ = std::max(accumulator_threshold_, 1.0); - if( debug_view_) { + if (debug_view_) + { // https://github.com/Itseez/opencv/blob/2.4.8/modules/imgproc/src/hough.cpp#L817 - cv::Canny(frame, edges, MAX(canny_threshold_/2,1), canny_threshold_, 3 ); + cv::Canny(frame, edges, MAX(canny_threshold_ / 2, 1), canny_threshold_, 3); } - if ( min_distance_between_circles_ == 0 ) { // set inital value - min_distance_between_circles_ = src_gray.rows/8; + if (min_distance_between_circles_ == 0) + { // set inital value + min_distance_between_circles_ = src_gray.rows / 8; config_.min_distance_between_circles = min_distance_between_circles_; reconfigure_server_->updateConfig(config_); } - //runs the detection, and update the display + // runs the detection, and update the display // will hold the results of the detection std::vector circles; // runs the actual detection - cv::HoughCircles( src_gray, circles, - CV_HOUGH_GRADIENT, - dp_, - min_distance_between_circles_, - canny_threshold_, - accumulator_threshold_, - min_circle_radius_, - max_circle_radius_ ); + cv::HoughCircles(src_gray, circles, CV_HOUGH_GRADIENT, dp_, min_distance_between_circles_, canny_threshold_, + accumulator_threshold_, min_circle_radius_, max_circle_radius_); cv::Mat out_image; - if ( frame.channels() == 1 ) { - cv::cvtColor( frame, out_image, cv::COLOR_GRAY2BGR); - } else { + if (frame.channels() == 1) + { + cv::cvtColor(frame, out_image, cv::COLOR_GRAY2BGR); + } + else + { out_image = frame; } // clone the colour, input image for displaying purposes - for( size_t i = 0; i < circles.size(); i++ ) + for (size_t i = 0; i < circles.size(); i++) { cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1])); int radius = cvRound(circles[i][2]); // circle center - circle( out_image, center, 3, cv::Scalar(0,255,0), -1, 8, 0 ); + circle(out_image, center, 3, cv::Scalar(0, 255, 0), -1, 8, 0); // circle outline - circle( out_image, center, radius, cv::Scalar(0,0,255), 3, 8, 0 ); + circle(out_image, center, radius, cv::Scalar(0, 0, 255), 3, 8, 0); opencv_apps::Circle circle_msg; circle_msg.center.x = center.x; @@ -250,9 +267,11 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet } // shows the results - if( debug_view_ || debug_image_pub_.getNumSubscribers() > 0 ) { + if (debug_view_ || debug_image_pub_.getNumSubscribers() > 0) + { cv::Mat debug_image; - switch (debug_image_type_) { + switch (debug_image_type_) + { case 1: debug_image = src_gray; break; @@ -263,17 +282,21 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet debug_image = frame; break; } - if ( debug_view_ ) { - cv::imshow( window_name_, debug_image ); + if (debug_view_) + { + cv::imshow(window_name_, debug_image); int c = cv::waitKey(1); - if ( c == 's' ) { - debug_image_type_ = (++debug_image_type_)%3; + if (c == 's') + { + debug_image_type_ = (++debug_image_type_) % 3; config_.debug_image_type = debug_image_type_; reconfigure_server_->updateConfig(config_); } } - if ( debug_image_pub_.getNumSubscribers() > 0 ) { - sensor_msgs::Image::Ptr out_debug_img = cv_bridge::CvImage(msg->header, msg->encoding, debug_image).toImageMsg(); + if (debug_image_pub_.getNumSubscribers() > 0) + { + sensor_msgs::Image::Ptr out_debug_img = + cv_bridge::CvImage(msg->header, msg->encoding, debug_image).toImageMsg(); debug_image_pub_.publish(out_debug_img); } } @@ -283,7 +306,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet img_pub_.publish(out_img); msg_pub_.publish(circles_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -316,7 +339,8 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet debug_image_type_ = 0; pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = debug_view_; } prev_stamp_ = ros::Time(0, 0); @@ -328,13 +352,13 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet max_canny_threshold_ = 255; min_distance_between_circles_ = 0; - //declare and initialize both parameters that are subjects to change + // declare and initialize both parameters that are subjects to change canny_threshold_ = canny_threshold_initial_value_; accumulator_threshold_ = accumulator_threshold_initial_value_; reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&HoughCirclesNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&HoughCirclesNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -347,19 +371,21 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet } }; bool HoughCirclesNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace hough_circles { -class HoughCirclesNodelet : public opencv_apps::HoughCirclesNodelet { +namespace hough_circles +{ +class HoughCirclesNodelet : public opencv_apps::HoughCirclesNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet hough_circles/hough_circles is deprecated, " "and renamed to opencv_apps/hough_circles."); opencv_apps::HoughCirclesNodelet::onInit(); } }; -} // namespace hough_circles - +} // namespace hough_circles #include PLUGINLIB_EXPORT_CLASS(opencv_apps::HoughCirclesNodelet, nodelet::Nodelet); diff --git a/src/nodelet/hough_lines_nodelet.cpp b/src/nodelet/hough_lines_nodelet.cpp index fb203a95..e50ae958 100644 --- a/src/nodelet/hough_lines_nodelet.cpp +++ b/src/nodelet/hough_lines_nodelet.cpp @@ -55,7 +55,8 @@ #include "opencv_apps/LineArray.h" #include "opencv_apps/LineArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class HoughLinesNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -85,17 +86,17 @@ class HoughLinesNodelet : public opencv_apps::Nodelet double minLineLength_; double maxLineGap_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { - config_ = new_config; - rho_ = config_.rho; - theta_ = config_.theta; - threshold_ = config_.threshold; - minLineLength_ = config_.minLineLength; - maxLineGap_ = config_.maxLineGap; + config_ = new_config; + rho_ = config_.rho; + theta_ = config_.theta; + threshold_ = config_.threshold; + minLineLength_ = config_.minLineLength; + maxLineGap_ = config_.maxLineGap; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -112,7 +113,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -126,29 +127,35 @@ class HoughLinesNodelet : public opencv_apps::Nodelet cv::Mat in_image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; cv::Mat src_gray; - if (in_image.channels() > 1) { - cv::cvtColor( in_image, src_gray, cv::COLOR_BGR2GRAY ); + if (in_image.channels() > 1) + { + cv::cvtColor(in_image, src_gray, cv::COLOR_BGR2GRAY); /// Apply Canny edge detector - Canny( src_gray, in_image, 50, 200, 3 ); + Canny(src_gray, in_image, 50, 200, 3); } - else { + else + { /// Check whether input gray image is filtered such that canny, sobel ...etc bool is_filtered = true; - for(int y=0; y < in_image.rows; ++y) { - for(int x=0; x < in_image.cols; ++x) { - if(!(in_image.at(y, x) == 0 - || in_image.at(y, x) == 255)) { + for (int y = 0; y < in_image.rows; ++y) + { + for (int x = 0; x < in_image.cols; ++x) + { + if (!(in_image.at(y, x) == 0 || in_image.at(y, x) == 255)) + { is_filtered = false; break; } - if(!is_filtered) { + if (!is_filtered) + { break; } } } - if(!is_filtered) { - Canny( in_image, in_image, 50, 200, 3 ); + if (!is_filtered) + { + Canny(in_image, in_image, 50, 200, 3); } } @@ -162,85 +169,89 @@ class HoughLinesNodelet : public opencv_apps::Nodelet // Do the work std::vector faces; - if( debug_view_) { + if (debug_view_) + { /// Create Trackbars for Thresholds char thresh_label[50]; - sprintf( thresh_label, "Thres: %d + input", min_threshold_ ); + sprintf(thresh_label, "Thres: %d + input", min_threshold_); - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); - cv::createTrackbar( thresh_label, window_name_, &threshold_, max_threshold_, trackbarCallback); - if (need_config_update_) { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + cv::createTrackbar(thresh_label, window_name_, &threshold_, max_threshold_, trackbarCallback); + if (need_config_update_) + { config_.threshold = threshold_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } - switch (config_.hough_type) { + switch (config_.hough_type) + { case opencv_apps::HoughLines_Standard_Hough_Transform: - { - std::vector s_lines; + { + std::vector s_lines; - /// 1. Use Standard Hough Transform - cv::HoughLines( in_image, s_lines, rho_, theta_ * CV_PI/180, threshold_, minLineLength_, maxLineGap_ ); + /// 1. Use Standard Hough Transform + cv::HoughLines(in_image, s_lines, rho_, theta_ * CV_PI / 180, threshold_, minLineLength_, maxLineGap_); - /// Show the result - for( size_t i = 0; i < s_lines.size(); i++ ) - { - float r = s_lines[i][0], t = s_lines[i][1]; - double cos_t = cos(t), sin_t = sin(t); - double x0 = r*cos_t, y0 = r*sin_t; - double alpha = 1000; + /// Show the result + for (size_t i = 0; i < s_lines.size(); i++) + { + float r = s_lines[i][0], t = s_lines[i][1]; + double cos_t = cos(t), sin_t = sin(t); + double x0 = r * cos_t, y0 = r * sin_t; + double alpha = 1000; - cv::Point pt1( cvRound(x0 + alpha*(-sin_t)), cvRound(y0 + alpha*cos_t) ); - cv::Point pt2( cvRound(x0 - alpha*(-sin_t)), cvRound(y0 - alpha*cos_t) ); + cv::Point pt1(cvRound(x0 + alpha * (-sin_t)), cvRound(y0 + alpha * cos_t)); + cv::Point pt2(cvRound(x0 - alpha * (-sin_t)), cvRound(y0 - alpha * cos_t)); #ifndef CV_VERSION_EPOCH - cv::line( out_image, pt1, pt2, cv::Scalar(255,0,0), 3, cv::LINE_AA); + cv::line(out_image, pt1, pt2, cv::Scalar(255, 0, 0), 3, cv::LINE_AA); #else - cv::line( out_image, pt1, pt2, cv::Scalar(255,0,0), 3, CV_AA); + cv::line(out_image, pt1, pt2, cv::Scalar(255, 0, 0), 3, CV_AA); #endif - opencv_apps::Line line_msg; - line_msg.pt1.x = pt1.x; - line_msg.pt1.y = pt1.y; - line_msg.pt2.x = pt2.x; - line_msg.pt2.y = pt2.y; - lines_msg.lines.push_back(line_msg); - } - - break; + opencv_apps::Line line_msg; + line_msg.pt1.x = pt1.x; + line_msg.pt1.y = pt1.y; + line_msg.pt2.x = pt2.x; + line_msg.pt2.y = pt2.y; + lines_msg.lines.push_back(line_msg); } + + break; + } case opencv_apps::HoughLines_Probabilistic_Hough_Transform: default: - { - std::vector p_lines; + { + std::vector p_lines; - /// 2. Use Probabilistic Hough Transform - cv::HoughLinesP( in_image, p_lines, rho_, theta_ * CV_PI/180, threshold_, minLineLength_, maxLineGap_ ); + /// 2. Use Probabilistic Hough Transform + cv::HoughLinesP(in_image, p_lines, rho_, theta_ * CV_PI / 180, threshold_, minLineLength_, maxLineGap_); - /// Show the result - for( size_t i = 0; i < p_lines.size(); i++ ) - { - cv::Vec4i l = p_lines[i]; + /// Show the result + for (size_t i = 0; i < p_lines.size(); i++) + { + cv::Vec4i l = p_lines[i]; #ifndef CV_VERSION_EPOCH - cv::line( out_image, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255,0,0), 3, cv::LINE_AA); + cv::line(out_image, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255, 0, 0), 3, cv::LINE_AA); #else - cv::line( out_image, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255,0,0), 3, CV_AA); + cv::line(out_image, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255, 0, 0), 3, CV_AA); #endif - opencv_apps::Line line_msg; - line_msg.pt1.x = l[0]; - line_msg.pt1.y = l[1]; - line_msg.pt2.x = l[2]; - line_msg.pt2.y = l[3]; - lines_msg.lines.push_back(line_msg); - } - - break; + opencv_apps::Line line_msg; + line_msg.pt1.x = l[0]; + line_msg.pt1.y = l[1]; + line_msg.pt2.x = l[2]; + line_msg.pt2.y = l[3]; + lines_msg.lines.push_back(line_msg); } + + break; + } } //-- Show what you got - if( debug_view_) { - cv::imshow( window_name_, out_image ); + if (debug_view_) + { + cv::imshow(window_name_, out_image); int c = cv::waitKey(1); } @@ -249,7 +260,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet img_pub_.publish(out_img); msg_pub_.publish(lines_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -281,7 +292,8 @@ class HoughLinesNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -293,7 +305,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&HoughLinesNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&HoughLinesNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -303,19 +315,21 @@ class HoughLinesNodelet : public opencv_apps::Nodelet } }; bool HoughLinesNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace hough_lines { -class HoughLinesNodelet : public opencv_apps::HoughLinesNodelet { +namespace hough_lines +{ +class HoughLinesNodelet : public opencv_apps::HoughLinesNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet hough_lines/hough_lines is deprecated, " "and renamed to opencv_apps/hough_lines."); opencv_apps::HoughLinesNodelet::onInit(); } }; -} // namespace hough_lines - +} // namespace hough_lines #include PLUGINLIB_EXPORT_CLASS(opencv_apps::HoughLinesNodelet, nodelet::Nodelet); diff --git a/src/nodelet/lk_flow_nodelet.cpp b/src/nodelet/lk_flow_nodelet.cpp index 8e893225..f08b3f53 100644 --- a/src/nodelet/lk_flow_nodelet.cpp +++ b/src/nodelet/lk_flow_nodelet.cpp @@ -53,7 +53,8 @@ #include "opencv_apps/LKFlowConfig.h" #include "opencv_apps/FlowArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class LKFlowNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -91,7 +92,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet int block_size_; float harris_k_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; quality_level_ = config_.quality_level; @@ -100,7 +101,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet harris_k_ = config_.harris_k; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -126,7 +127,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet } } #endif - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -143,15 +144,17 @@ class LKFlowNodelet : public opencv_apps::Nodelet opencv_apps::FlowArrayStamped flows_msg; flows_msg.header = msg->header; - if( debug_view_) { + if (debug_view_) + { /// Create Trackbars for Thresholds - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); cv::createTrackbar("Min Distance", window_name_, &min_distance_, 100, trackbarCallback); cv::createTrackbar("Block Size", window_name_, &block_size_, 100, trackbarCallback); - //cv::setMouseCallback( window_name_, onMouse, 0 ); - if (need_config_update_) { + // cv::setMouseCallback( window_name_, onMouse, 0 ); + if (need_config_update_) + { reconfigure_server_->updateConfig(config_); config_.quality_level = quality_level_; config_.min_distance = min_distance_; @@ -163,58 +166,61 @@ class LKFlowNodelet : public opencv_apps::Nodelet // Do the work cv::TermCriteria termcrit(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 20, 0.03); - cv::Size subPixWinSize(10,10), winSize(31,31); + cv::Size subPixWinSize(10, 10), winSize(31, 31); - if ( image.channels() > 1 ) { - cv::cvtColor( image, gray, cv::COLOR_BGR2GRAY ); - } else { + if (image.channels() > 1) + { + cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY); + } + else + { image.copyTo(gray); } - if( nightMode ) + if (nightMode) image = cv::Scalar::all(0); - if( needToInit ) + if (needToInit) { // automatic initialization - cv::goodFeaturesToTrack(gray, points[1], MAX_COUNT, quality_level_, min_distance_, cv::Mat(), block_size_, false, harris_k_); - cv::cornerSubPix(gray, points[1], subPixWinSize, cv::Size(-1,-1), termcrit); + cv::goodFeaturesToTrack(gray, points[1], MAX_COUNT, quality_level_, min_distance_, cv::Mat(), block_size_, + false, harris_k_); + cv::cornerSubPix(gray, points[1], subPixWinSize, cv::Size(-1, -1), termcrit); addRemovePt = false; } - else if( !points[0].empty() ) + else if (!points[0].empty()) { std::vector status; std::vector err; - if(prevGray.empty()) + if (prevGray.empty()) gray.copyTo(prevGray); - cv::calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize, - 3, termcrit, 0, 0.001); + cv::calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize, 3, termcrit, 0, 0.001); size_t i, k; - for( i = k = 0; i < points[1].size(); i++ ) + for (i = k = 0; i < points[1].size(); i++) { - if( addRemovePt ) + if (addRemovePt) { - if( cv::norm(point - points[1][i]) <= 5 ) + if (cv::norm(point - points[1][i]) <= 5) { addRemovePt = false; continue; } } - if( !status[i] ) + if (!status[i]) continue; points[1][k++] = points[1][i]; - cv::circle( image, points[1][i], 3, cv::Scalar(0,255,0), -1, 8); - cv::line( image, points[1][i], points[0][i], cv::Scalar(0,255,0), 1, 8, 0); + cv::circle(image, points[1][i], 3, cv::Scalar(0, 255, 0), -1, 8); + cv::line(image, points[1][i], points[0][i], cv::Scalar(0, 255, 0), 1, 8, 0); opencv_apps::Flow flow_msg; opencv_apps::Point2D point_msg; opencv_apps::Point2D velocity_msg; point_msg.x = points[1][i].x; point_msg.y = points[1][i].y; - velocity_msg.x = points[1][i].x-points[0][i].x; - velocity_msg.y = points[1][i].y-points[0][i].y; + velocity_msg.x = points[1][i].x - points[0][i].x; + velocity_msg.y = points[1][i].y - points[0][i].y; flow_msg.point = point_msg; flow_msg.velocity = velocity_msg; flows_msg.flow.push_back(flow_msg); @@ -222,24 +228,24 @@ class LKFlowNodelet : public opencv_apps::Nodelet points[1].resize(k); } - if( addRemovePt && points[1].size() < (size_t)MAX_COUNT ) - { - std::vector tmp; - tmp.push_back(point); - cv::cornerSubPix( gray, tmp, winSize, cv::Size(-1,-1), termcrit); - points[1].push_back(tmp[0]); - addRemovePt = false; - } + if (addRemovePt && points[1].size() < (size_t)MAX_COUNT) + { + std::vector tmp; + tmp.push_back(point); + cv::cornerSubPix(gray, tmp, winSize, cv::Size(-1, -1), termcrit); + points[1].push_back(tmp[0]); + addRemovePt = false; + } needToInit = false; - if( debug_view_) { - + if (debug_view_) + { cv::imshow(window_name_, image); char c = (char)cv::waitKey(1); - //if( c == 27 ) + // if( c == 27 ) // break; - switch( c ) + switch (c) { case 'r': needToInit = true; @@ -261,7 +267,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet img_pub_.publish(out_img); msg_pub_.publish(flows_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -269,18 +275,21 @@ class LKFlowNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - bool initialize_points_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { + bool initialize_points_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) + { needToInit = true; return true; } - bool delete_points_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { + bool delete_points_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) + { points[0].clear(); points[1].clear(); return true; } - bool toggle_night_mode_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { + bool toggle_night_mode_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) + { nightMode = !nightMode; return true; } @@ -309,7 +318,8 @@ class LKFlowNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -322,40 +332,43 @@ class LKFlowNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&LKFlowNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&LKFlowNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); - + img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "flows", 1); - initialize_points_service_ = pnh_->advertiseService("initialize_points", &LKFlowNodelet::initialize_points_cb, this); + initialize_points_service_ = + pnh_->advertiseService("initialize_points", &LKFlowNodelet::initialize_points_cb, this); delete_points_service_ = pnh_->advertiseService("delete_points", &LKFlowNodelet::delete_points_cb, this); - toggle_night_mode_service_ = pnh_->advertiseService("toggle_night_mode", &LKFlowNodelet::toggle_night_mode_cb, this); - + toggle_night_mode_service_ = + pnh_->advertiseService("toggle_night_mode", &LKFlowNodelet::toggle_night_mode_cb, this); NODELET_INFO("Hot keys: "); NODELET_INFO("\tESC - quit the program"); NODELET_INFO("\tr - auto-initialize tracking"); NODELET_INFO("\tc - delete all the points"); NODELET_INFO("\tn - switch the \"night\" mode on/off"); - //NODELET_INFO("To add/remove a feature point click it"); + // NODELET_INFO("To add/remove a feature point click it"); onInitPostProcess(); } }; bool LKFlowNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace lk_flow { -class LKFlowNodelet : public opencv_apps::LKFlowNodelet { +namespace lk_flow +{ +class LKFlowNodelet : public opencv_apps::LKFlowNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet lk_flow/lk_flow is deprecated, " "and renamed to opencv_apps/lk_flow."); opencv_apps::LKFlowNodelet::onInit(); } }; -} // namespace lk_flow - +} // namespace lk_flow #include PLUGINLIB_EXPORT_CLASS(opencv_apps::LKFlowNodelet, nodelet::Nodelet); diff --git a/src/nodelet/nodelet.cpp b/src/nodelet/nodelet.cpp index 5b8bdd50..af5b30a6 100644 --- a/src/nodelet/nodelet.cpp +++ b/src/nodelet/nodelet.cpp @@ -37,148 +37,172 @@ namespace opencv_apps { - void Nodelet::onInit() +void Nodelet::onInit() +{ + connection_status_ = NOT_SUBSCRIBED; + nh_.reset(new ros::NodeHandle(getMTNodeHandle())); + pnh_.reset(new ros::NodeHandle(getMTPrivateNodeHandle())); + pnh_->param("always_subscribe", always_subscribe_, false); + pnh_->param("verbose_connection", verbose_connection_, false); + if (!verbose_connection_) { - connection_status_ = NOT_SUBSCRIBED; - nh_.reset (new ros::NodeHandle (getMTNodeHandle ())); - pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ())); - pnh_->param("always_subscribe", always_subscribe_, false); - pnh_->param("verbose_connection", verbose_connection_, false); - if (!verbose_connection_) { - nh_->param("verbose_connection", verbose_connection_, false); - } - // timer to warn when no connection in a few seconds - ever_subscribed_ = false; - timer_ = nh_->createWallTimer( - ros::WallDuration(5), - &Nodelet::warnNeverSubscribedCallback, - this, - /*oneshot=*/true); + nh_->param("verbose_connection", verbose_connection_, false); } + // timer to warn when no connection in a few seconds + ever_subscribed_ = false; + timer_ = nh_->createWallTimer(ros::WallDuration(5), &Nodelet::warnNeverSubscribedCallback, this, + /*oneshot=*/true); +} - void Nodelet::onInitPostProcess() +void Nodelet::onInitPostProcess() +{ + if (always_subscribe_) { - if (always_subscribe_) { - subscribe(); - } + subscribe(); } +} - void Nodelet::warnNeverSubscribedCallback(const ros::WallTimerEvent& event) +void Nodelet::warnNeverSubscribedCallback(const ros::WallTimerEvent& event) +{ + if (!ever_subscribed_) { - if (!ever_subscribed_) { - NODELET_WARN("'%s' subscribes topics only with child subscribers.", nodelet::Nodelet::getName().c_str()); - } + NODELET_WARN("'%s' subscribes topics only with child subscribers.", nodelet::Nodelet::getName().c_str()); } +} - void Nodelet::connectionCallback(const ros::SingleSubscriberPublisher& pub) +void Nodelet::connectionCallback(const ros::SingleSubscriberPublisher& pub) +{ + if (verbose_connection_) { - if (verbose_connection_) { - NODELET_INFO("New connection or disconnection is detected"); - } - if (!always_subscribe_) { - boost::mutex::scoped_lock lock(connection_mutex_); - for (size_t i = 0; i < publishers_.size(); i++) { - ros::Publisher pub = publishers_[i]; - if (pub.getNumSubscribers() > 0) { - if (!ever_subscribed_) { - ever_subscribed_ = true; - } - if (connection_status_ != SUBSCRIBED) { - if (verbose_connection_) { - NODELET_INFO("Subscribe input topics"); - } - subscribe(); - connection_status_ = SUBSCRIBED; + NODELET_INFO("New connection or disconnection is detected"); + } + if (!always_subscribe_) + { + boost::mutex::scoped_lock lock(connection_mutex_); + for (size_t i = 0; i < publishers_.size(); i++) + { + ros::Publisher pub = publishers_[i]; + if (pub.getNumSubscribers() > 0) + { + if (!ever_subscribed_) + { + ever_subscribed_ = true; + } + if (connection_status_ != SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Subscribe input topics"); } - return; + subscribe(); + connection_status_ = SUBSCRIBED; } + return; } - if (connection_status_ == SUBSCRIBED) { - if (verbose_connection_) { - NODELET_INFO("Unsubscribe input topics"); - } - unsubscribe(); - connection_status_ = NOT_SUBSCRIBED; + } + if (connection_status_ == SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Unsubscribe input topics"); } + unsubscribe(); + connection_status_ = NOT_SUBSCRIBED; } } - - void Nodelet::imageConnectionCallback( - const image_transport::SingleSubscriberPublisher& pub) +} + +void Nodelet::imageConnectionCallback(const image_transport::SingleSubscriberPublisher& pub) +{ + if (verbose_connection_) { - if (verbose_connection_) { - NODELET_INFO("New image connection or disconnection is detected"); - } - if (!always_subscribe_) { - boost::mutex::scoped_lock lock(connection_mutex_); - for (size_t i = 0; i < image_publishers_.size(); i++) { - image_transport::Publisher pub = image_publishers_[i]; - if (pub.getNumSubscribers() > 0) { - if (!ever_subscribed_) { - ever_subscribed_ = true; - } - if (connection_status_ != SUBSCRIBED) { - if (verbose_connection_) { - NODELET_INFO("Subscribe input topics"); - } - subscribe(); - connection_status_ = SUBSCRIBED; + NODELET_INFO("New image connection or disconnection is detected"); + } + if (!always_subscribe_) + { + boost::mutex::scoped_lock lock(connection_mutex_); + for (size_t i = 0; i < image_publishers_.size(); i++) + { + image_transport::Publisher pub = image_publishers_[i]; + if (pub.getNumSubscribers() > 0) + { + if (!ever_subscribed_) + { + ever_subscribed_ = true; + } + if (connection_status_ != SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Subscribe input topics"); } - return; + subscribe(); + connection_status_ = SUBSCRIBED; } + return; } - if (connection_status_ == SUBSCRIBED) { - if (verbose_connection_) { - NODELET_INFO("Unsubscribe input topics"); - } - unsubscribe(); - connection_status_ = NOT_SUBSCRIBED; + } + if (connection_status_ == SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Unsubscribe input topics"); } + unsubscribe(); + connection_status_ = NOT_SUBSCRIBED; } } +} - void Nodelet::cameraConnectionCallback( - const image_transport::SingleSubscriberPublisher& pub) - { - cameraConnectionBaseCallback(); - } +void Nodelet::cameraConnectionCallback(const image_transport::SingleSubscriberPublisher& pub) +{ + cameraConnectionBaseCallback(); +} - void Nodelet::cameraInfoConnectionCallback( - const ros::SingleSubscriberPublisher& pub) +void Nodelet::cameraInfoConnectionCallback(const ros::SingleSubscriberPublisher& pub) +{ + cameraConnectionBaseCallback(); +} + +void Nodelet::cameraConnectionBaseCallback() +{ + if (verbose_connection_) { - cameraConnectionBaseCallback(); + NODELET_INFO("New image connection or disconnection is detected"); } - - void Nodelet::cameraConnectionBaseCallback() + if (!always_subscribe_) { - if (verbose_connection_) { - NODELET_INFO("New image connection or disconnection is detected"); - } - if (!always_subscribe_) { - boost::mutex::scoped_lock lock(connection_mutex_); - for (size_t i = 0; i < camera_publishers_.size(); i++) { - image_transport::CameraPublisher pub = camera_publishers_[i]; - if (pub.getNumSubscribers() > 0) { - if (!ever_subscribed_) { - ever_subscribed_ = true; - } - if (connection_status_ != SUBSCRIBED) { - if (verbose_connection_) { - NODELET_INFO("Subscribe input topics"); - } - subscribe(); - connection_status_ = SUBSCRIBED; + boost::mutex::scoped_lock lock(connection_mutex_); + for (size_t i = 0; i < camera_publishers_.size(); i++) + { + image_transport::CameraPublisher pub = camera_publishers_[i]; + if (pub.getNumSubscribers() > 0) + { + if (!ever_subscribed_) + { + ever_subscribed_ = true; + } + if (connection_status_ != SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Subscribe input topics"); } - return; + subscribe(); + connection_status_ = SUBSCRIBED; } + return; } - if (connection_status_ == SUBSCRIBED) { - if (verbose_connection_) { - NODELET_INFO("Unsubscribe input topics"); - } - unsubscribe(); - connection_status_ = NOT_SUBSCRIBED; + } + if (connection_status_ == SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Unsubscribe input topics"); } + unsubscribe(); + connection_status_ = NOT_SUBSCRIBED; } } } +} diff --git a/src/nodelet/people_detect_nodelet.cpp b/src/nodelet/people_detect_nodelet.cpp index fc0f0f05..8ac6e42e 100644 --- a/src/nodelet/people_detect_nodelet.cpp +++ b/src/nodelet/people_detect_nodelet.cpp @@ -3,11 +3,11 @@ * * Copyright (c) 2014, Kei Okada. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -53,7 +53,8 @@ #include "opencv_apps/Rect.h" #include "opencv_apps/RectArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class PeopleDetectNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -83,7 +84,7 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet double scale0_; int group_threshold_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; hit_threshold_ = config_.hit_threshold; @@ -93,7 +94,7 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet group_threshold_ = config_.group_threshold; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -104,13 +105,13 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet { do_work(msg, cam_info->header.frame_id); } - + void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -128,8 +129,9 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet found_msg.header = msg->header; // Do the work - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); } std::vector found, found_filtered; @@ -137,29 +139,30 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet // run the detector with default parameters. to get a higher hit-rate // (and more false alarms, respectively), decrease the hitThreshold and // groupThreshold (set groupThreshold to 0 to turn off the grouping completely). - hog_.detectMultiScale(frame, found, hit_threshold_, cv::Size(win_stride_, win_stride_), cv::Size(padding_, padding_), scale0_, group_threshold_); + hog_.detectMultiScale(frame, found, hit_threshold_, cv::Size(win_stride_, win_stride_), + cv::Size(padding_, padding_), scale0_, group_threshold_); t = (double)cv::getTickCount() - t; - NODELET_INFO("tdetection time = %gms", t*1000./cv::getTickFrequency()); + NODELET_INFO("tdetection time = %gms", t * 1000. / cv::getTickFrequency()); size_t i, j; - for( i = 0; i < found.size(); i++ ) + for (i = 0; i < found.size(); i++) { cv::Rect r = found[i]; - for( j = 0; j < found.size(); j++ ) - if( j != i && (r & found[j]) == r) + for (j = 0; j < found.size(); j++) + if (j != i && (r & found[j]) == r) break; - if( j == found.size() ) + if (j == found.size()) found_filtered.push_back(r); } - for( i = 0; i < found_filtered.size(); i++ ) + for (i = 0; i < found_filtered.size(); i++) { cv::Rect r = found_filtered[i]; // the HOG detector returns slightly larger rectangles than the real objects. // so we slightly shrink the rectangles to get a nicer output. - r.x += cvRound(r.width*0.1); - r.width = cvRound(r.width*0.8); - r.y += cvRound(r.height*0.07); - r.height = cvRound(r.height*0.8); - cv::rectangle(frame, r.tl(), r.br(), cv::Scalar(0,255,0), 3); + r.x += cvRound(r.width * 0.1); + r.width = cvRound(r.width * 0.8); + r.y += cvRound(r.height * 0.07); + r.height = cvRound(r.height * 0.8); + cv::rectangle(frame, r.tl(), r.br(), cv::Scalar(0, 255, 0), 3); opencv_apps::Rect rect_msg; rect_msg.x = r.x; @@ -170,17 +173,18 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet } //-- Show what you got - if( debug_view_) { - cv::imshow( window_name_, frame ); + if (debug_view_) + { + cv::imshow(window_name_, frame); int c = cv::waitKey(1); } // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg(); + sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(found_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -212,7 +216,8 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -221,11 +226,11 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&PeopleDetectNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&PeopleDetectNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); hog_.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector()); - + img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "found", 1); @@ -233,19 +238,21 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet } }; bool PeopleDetectNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace people_detect { -class PeopleDetectNodelet : public opencv_apps::PeopleDetectNodelet { +namespace people_detect +{ +class PeopleDetectNodelet : public opencv_apps::PeopleDetectNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet people_detect/people_detect is deprecated, " "and renamed to opencv_apps/people_detect."); opencv_apps::PeopleDetectNodelet::onInit(); } }; -} // namespace people_detect - +} // namespace people_detect #include PLUGINLIB_EXPORT_CLASS(opencv_apps::PeopleDetectNodelet, nodelet::Nodelet); diff --git a/src/nodelet/phase_corr_nodelet.cpp b/src/nodelet/phase_corr_nodelet.cpp index 0cc4f8a9..8455f108 100644 --- a/src/nodelet/phase_corr_nodelet.cpp +++ b/src/nodelet/phase_corr_nodelet.cpp @@ -47,7 +47,8 @@ #include "opencv_apps/PhaseCorrConfig.h" #include "opencv_apps/Point2DStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class PhaseCorrNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -71,12 +72,12 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet std::string window_name_; static bool need_config_update_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -93,7 +94,7 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -111,21 +112,26 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet shift_msg.header = msg->header; // Do the work - if ( frame.channels() > 1 ) { - cv::cvtColor( frame, curr, cv::COLOR_BGR2GRAY ); - } else { + if (frame.channels() > 1) + { + cv::cvtColor(frame, curr, cv::COLOR_BGR2GRAY); + } + else + { curr = frame; } - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); - if (need_config_update_) { + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + if (need_config_update_) + { reconfigure_server_->updateConfig(config_); need_config_update_ = false; } } - if(prev.empty()) + if (prev.empty()) { prev = curr.clone(); cv::createHanningWindow(hann, curr.size(), CV_64F); @@ -135,18 +141,20 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet curr.convertTo(curr64f, CV_64F); cv::Point2d shift = cv::phaseCorrelate(prev64f, curr64f, hann); - double radius = cv::sqrt(shift.x*shift.x + shift.y*shift.y); + double radius = cv::sqrt(shift.x * shift.x + shift.y * shift.y); - if(radius > 0) + if (radius > 0) { // draw a circle and line indicating the shift direction... cv::Point center(curr.cols >> 1, curr.rows >> 1); #ifndef CV_VERSION_EPOCH - cv::circle(frame, center, (int)(radius*5), cv::Scalar(0, 255, 0), 3, cv::LINE_AA); - cv::line(frame, center, cv::Point(center.x + (int)(shift.x*5), center.y + (int)(shift.y*5)), cv::Scalar(0, 255, 0), 3, cv::LINE_AA); + cv::circle(frame, center, (int)(radius * 5), cv::Scalar(0, 255, 0), 3, cv::LINE_AA); + cv::line(frame, center, cv::Point(center.x + (int)(shift.x * 5), center.y + (int)(shift.y * 5)), + cv::Scalar(0, 255, 0), 3, cv::LINE_AA); #else - cv::circle(frame, center, (int)(radius*5), cv::Scalar(0, 255, 0), 3, CV_AA); - cv::line(frame, center, cv::Point(center.x + (int)(shift.x*5), center.y + (int)(shift.y*5)), cv::Scalar(0, 255, 0), 3, CV_AA); + cv::circle(frame, center, (int)(radius * 5), cv::Scalar(0, 255, 0), 3, CV_AA); + cv::line(frame, center, cv::Point(center.x + (int)(shift.x * 5), center.y + (int)(shift.y * 5)), + cv::Scalar(0, 255, 0), 3, CV_AA); #endif // @@ -155,20 +163,20 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet } //-- Show what you got - if( debug_view_) { - cv::imshow( window_name_, frame ); + if (debug_view_) + { + cv::imshow(window_name_, frame); int c = cv::waitKey(1); } prev = curr.clone(); - // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg(); + sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(shift_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -200,16 +208,17 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); window_name_ = "phase shift"; - + reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&PhaseCorrNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&PhaseCorrNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -219,19 +228,21 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet } }; bool PhaseCorrNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace phase_corr { -class PhaseCorrNodelet : public opencv_apps::PhaseCorrNodelet { +namespace phase_corr +{ +class PhaseCorrNodelet : public opencv_apps::PhaseCorrNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet phase_corr/phase_corr is deprecated, " "and renamed to opencv_apps/phase_corr."); opencv_apps::PhaseCorrNodelet::onInit(); } }; -} // namespace phase_corr - +} // namespace phase_corr #include PLUGINLIB_EXPORT_CLASS(opencv_apps::PhaseCorrNodelet, nodelet::Nodelet); diff --git a/src/nodelet/pyramids_nodelet.cpp b/src/nodelet/pyramids_nodelet.cpp index 65c1e7f7..b72c29b4 100644 --- a/src/nodelet/pyramids_nodelet.cpp +++ b/src/nodelet/pyramids_nodelet.cpp @@ -49,12 +49,13 @@ #include #include "opencv2/imgproc/imgproc.hpp" - #include "opencv_apps/PyramidsConfig.h" #include -namespace opencv_apps { -class PyramidsNodelet : public opencv_apps::Nodelet { +namespace opencv_apps +{ +class PyramidsNodelet : public opencv_apps::Nodelet +{ image_transport::Publisher img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; @@ -74,13 +75,14 @@ class PyramidsNodelet : public opencv_apps::Nodelet { std::string window_name_; - void reconfigureCallback(Config& new_config, uint32_t level) { + void reconfigureCallback(Config& new_config, uint32_t level) + { config_ = new_config; num_of_pyramids_ = config_.num_of_pyramids; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -100,23 +102,28 @@ class PyramidsNodelet : public opencv_apps::Nodelet { void do_work(const sensor_msgs::ImageConstPtr& image_msg, const std::string input_frame_from_msg) { // Work on the image. - try { + try + { // Convert the image into something opencv can handle. - cv::Mat src_image = - cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; + cv::Mat src_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; // Do the work int num = num_of_pyramids_; - switch (config_.pyramids_type) { - case opencv_apps::Pyramids_Up: { - while(num) { + switch (config_.pyramids_type) + { + case opencv_apps::Pyramids_Up: + { + while (num) + { num--; cv::pyrUp(src_image, src_image, cv::Size(src_image.cols * 2, src_image.rows * 2)); } break; } - case opencv_apps::Pyramids_Down: { - while(num) { + case opencv_apps::Pyramids_Down: + { + while (num) + { num--; cv::pyrDown(src_image, src_image, cv::Size(src_image.cols / 2, src_image.rows / 2)); } @@ -125,18 +132,18 @@ class PyramidsNodelet : public opencv_apps::Nodelet { } //-- Show what you got - if(debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); cv::imshow(window_name_, src_image); int c = cv::waitKey(1); } // Publish the image. - img_pub_.publish(cv_bridge::CvImage(image_msg->header, - image_msg->encoding, - src_image).toImageMsg()); + img_pub_.publish(cv_bridge::CvImage(image_msg->header, image_msg->encoding, src_image).toImageMsg()); } - catch (cv::Exception &e) { + catch (cv::Exception& e) + { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } } @@ -158,14 +165,16 @@ class PyramidsNodelet : public opencv_apps::Nodelet { } public: - virtual void onInit() { + virtual void onInit() + { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } @@ -173,7 +182,7 @@ class PyramidsNodelet : public opencv_apps::Nodelet { reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&PyramidsNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&PyramidsNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -181,19 +190,21 @@ class PyramidsNodelet : public opencv_apps::Nodelet { onInitPostProcess(); } }; -} // namespace opencv_apps +} // namespace opencv_apps -namespace pyramids { -class PyramidsNodelet : public opencv_apps::PyramidsNodelet { +namespace pyramids +{ +class PyramidsNodelet : public opencv_apps::PyramidsNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet pyramids/pyramids is deprecated, " "and renamed to opencv_apps/pyramids."); opencv_apps::PyramidsNodelet::onInit(); } }; -} // namespace pyramids - +} // namespace pyramids #include PLUGINLIB_EXPORT_CLASS(opencv_apps::PyramidsNodelet, nodelet::Nodelet); diff --git a/src/nodelet/segment_objects_nodelet.cpp b/src/nodelet/segment_objects_nodelet.cpp index d3fabd3e..359361d5 100644 --- a/src/nodelet/segment_objects_nodelet.cpp +++ b/src/nodelet/segment_objects_nodelet.cpp @@ -3,11 +3,11 @@ * * Copyright (c) 2014, Kei Okada. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -55,7 +55,8 @@ #include "opencv_apps/ContourArray.h" #include "opencv_apps/ContourArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class SegmentObjectsNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -85,12 +86,12 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet #endif bool update_bg_model; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -101,13 +102,13 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet { do_work(msg, cam_info->header.frame_id); } - + void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -127,10 +128,12 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet // Do the work cv::Mat bgmask, out_frame; - if( debug_view_) { + if (debug_view_) + { /// Create Trackbars for Thresholds - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); - if (need_config_update_) { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + if (need_config_update_) + { reconfigure_server_->updateConfig(config_); need_config_update_ = false; } @@ -141,7 +144,7 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet #else bgsubtractor(frame, bgmask, update_bg_model ? -1 : 0); #endif - //refineSegments(tmp_frame, bgmask, out_frame); + // refineSegments(tmp_frame, bgmask, out_frame); int niters = 3; std::vector > contours; @@ -149,15 +152,15 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet cv::Mat temp; - cv::dilate(bgmask, temp, cv::Mat(), cv::Point(-1,-1), niters); - cv::erode(temp, temp, cv::Mat(), cv::Point(-1,-1), niters*2); - cv::dilate(temp, temp, cv::Mat(), cv::Point(-1,-1), niters); + cv::dilate(bgmask, temp, cv::Mat(), cv::Point(-1, -1), niters); + cv::erode(temp, temp, cv::Mat(), cv::Point(-1, -1), niters * 2); + cv::dilate(temp, temp, cv::Mat(), cv::Point(-1, -1), niters); - cv::findContours( temp, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ); + cv::findContours(temp, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); out_frame = cv::Mat::zeros(frame.size(), CV_8UC3); - if( contours.size() == 0 ) + if (contours.size() == 0) return; // iterate through all the top-level contours, @@ -165,24 +168,26 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet int idx = 0, largestComp = 0; double maxArea = 0; - for( ; idx >= 0; idx = hierarchy[idx][0] ) + for (; idx >= 0; idx = hierarchy[idx][0]) { const std::vector& c = contours[idx]; double area = fabs(cv::contourArea(cv::Mat(c))); - if( area > maxArea ) + if (area > maxArea) { maxArea = area; largestComp = idx; } } - cv::Scalar color( 0, 0, 255 ); - cv::drawContours( out_frame, contours, largestComp, color, CV_FILLED, 8, hierarchy ); + cv::Scalar color(0, 0, 255); + cv::drawContours(out_frame, contours, largestComp, color, CV_FILLED, 8, hierarchy); std_msgs::Float64 area_msg; area_msg.data = maxArea; - for( size_t i = 0; i< contours.size(); i++ ) { + for (size_t i = 0; i < contours.size(); i++) + { opencv_apps::Contour contour_msg; - for ( size_t j = 0; j < contours[i].size(); j++ ) { + for (size_t j = 0; j < contours[i].size(); j++) + { opencv_apps::Point2D point_msg; point_msg.x = contours[i][j].x; point_msg.y = contours[i][j].y; @@ -192,35 +197,38 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet } //-- Show what you got - if( debug_view_) { - cv::imshow( window_name_, out_frame ); + if (debug_view_) + { + cv::imshow(window_name_, out_frame); int keycode = cv::waitKey(1); - //if( keycode == 27 ) + // if( keycode == 27 ) // break; - if( keycode == ' ' ) + if (keycode == ' ') { - update_bg_model = !update_bg_model; - NODELET_INFO("Learn background is in state = %d",update_bg_model); + update_bg_model = !update_bg_model; + NODELET_INFO("Learn background is in state = %d", update_bg_model); } } // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, out_frame).toImageMsg(); + sensor_msgs::Image::Ptr out_img = + cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, out_frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(contours_msg); area_pub_.publish(area_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } prev_stamp_ = msg->header.stamp; } - - bool update_bg_model_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { + + bool update_bg_model_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) + { update_bg_model = !update_bg_model; - NODELET_INFO("Learn background is in state = %d",update_bg_model); + NODELET_INFO("Learn background is in state = %d", update_bg_model); return true; } @@ -248,7 +256,8 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -264,31 +273,34 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&SegmentObjectsNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&SegmentObjectsNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); - + img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "contours", 1); area_pub_ = advertise(*pnh_, "area", 1); - update_bg_model_service_ = pnh_->advertiseService("update_bg_model", &SegmentObjectsNodelet::update_bg_model_cb, this); + update_bg_model_service_ = + pnh_->advertiseService("update_bg_model", &SegmentObjectsNodelet::update_bg_model_cb, this); onInitPostProcess(); } }; bool SegmentObjectsNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace segment_objects { -class SegmentObjectsNodelet : public opencv_apps::SegmentObjectsNodelet { +namespace segment_objects +{ +class SegmentObjectsNodelet : public opencv_apps::SegmentObjectsNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet segment_objects/segment_objects is deprecated, " "and renamed to opencv_apps/segment_objects."); opencv_apps::SegmentObjectsNodelet::onInit(); } }; -} // namespace segment_objects - +} // namespace segment_objects #include PLUGINLIB_EXPORT_CLASS(opencv_apps::SegmentObjectsNodelet, nodelet::Nodelet); diff --git a/src/nodelet/simple_compressed_example_nodelet.cpp b/src/nodelet/simple_compressed_example_nodelet.cpp index 1ea44c4d..0a940884 100644 --- a/src/nodelet/simple_compressed_example_nodelet.cpp +++ b/src/nodelet/simple_compressed_example_nodelet.cpp @@ -3,11 +3,11 @@ * * Copyright (c) 2015, Tal Regev. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -//http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages +// http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages /** * This is a demo of Simple Example from wiki tutorial */ @@ -48,13 +48,12 @@ #include #include - #include -namespace opencv_apps { - -namespace simple_compressed_example { - +namespace opencv_apps +{ +namespace simple_compressed_example +{ static const std::string OPENCV_WINDOW = "Image window"; class ImageConverter @@ -68,22 +67,23 @@ class ImageConverter public: ImageConverter() { - // Subscrive to input video feed and publish output video feed - image_sub_ = nh_.subscribe("image/compressed", queue_size_, - &ImageConverter::imageCb,this); + // Subscrive to input video feed and publish output video feed + image_sub_ = nh_.subscribe("image/compressed", queue_size_, &ImageConverter::imageCb, this); image_pub_ = nh_.advertise("/image_converter/output_video/compressed", 1); ros::NodeHandle pnh_("~"); pnh_.param("queue_size", queue_size_, 3); pnh_.param("debug_view", debug_view_, false); - if( debug_view_) { + if (debug_view_) + { cv::namedWindow(OPENCV_WINDOW); } } ~ImageConverter() { - if( debug_view_) { + if (debug_view_) + { cv::destroyWindow(OPENCV_WINDOW); } } @@ -93,49 +93,51 @@ class ImageConverter cv_bridge::CvImagePtr cv_ptr; try { - /* - Supporting CompressedImage in cv_bridge has been started from 1.11.9 (2015-11-29) - note : hydro 1.10.18, indigo : 1.11.13 ... - https://github.com/ros-perception/vision_opencv/pull/70 - */ +/* +Supporting CompressedImage in cv_bridge has been started from 1.11.9 (2015-11-29) +note : hydro 1.10.18, indigo : 1.11.13 ... +https://github.com/ros-perception/vision_opencv/pull/70 + */ #ifndef CV_BRIDGE_COMPRESSED_IMAGE_IS_NOT_SUPPORTED cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); #else - //cv_ptr = cv_bridge::toCvCopyImpl(matFromImage(msg), msg.header, sensor_msgs::image_encodings::BGR8, sensor_msgs::image_encodings::BGR8); - //cv::Mat matFromImage(const sensor_msgs::CompressedImage& source) - cv::Mat jpegData(1,msg->data.size(),CV_8UC1); - jpegData.data = const_cast(&msg->data[0]); - cv::InputArray data(jpegData); - cv::Mat image = cv::imdecode(data,cv::IMREAD_COLOR); - - //cv_ptr = cv_bridge::toCvCopyImpl(bgrMat, msg->header, sensor_msgs::image_encodings::BGR8, sensor_msgs::image_encodings::BGR8); - sensor_msgs::Image ros_image; - ros_image.header = msg->header; - ros_image.height = image.rows; - ros_image.width = image.cols; - ros_image.encoding = sensor_msgs::image_encodings::BGR8; - ros_image.is_bigendian = false; - ros_image.step = image.cols * image.elemSize(); - size_t size = ros_image.step * image.rows; - ros_image.data.resize(size); - - if (image.isContinuous()) - { - memcpy((char*)(&ros_image.data[0]), image.data, size); - } - else - { - // Copy by row by row - uchar* ros_data_ptr = (uchar*)(&ros_image.data[0]); - uchar* cv_data_ptr = image.data; - for (int i = 0; i < image.rows; ++i) - { - memcpy(ros_data_ptr, cv_data_ptr, ros_image.step); - ros_data_ptr += ros_image.step; - cv_data_ptr += image.step; - } - } - cv_ptr = cv_bridge::toCvCopy(ros_image, sensor_msgs::image_encodings::BGR8); + // cv_ptr = cv_bridge::toCvCopyImpl(matFromImage(msg), msg.header, sensor_msgs::image_encodings::BGR8, + // sensor_msgs::image_encodings::BGR8); + // cv::Mat matFromImage(const sensor_msgs::CompressedImage& source) + cv::Mat jpegData(1, msg->data.size(), CV_8UC1); + jpegData.data = const_cast(&msg->data[0]); + cv::InputArray data(jpegData); + cv::Mat image = cv::imdecode(data, cv::IMREAD_COLOR); + + // cv_ptr = cv_bridge::toCvCopyImpl(bgrMat, msg->header, sensor_msgs::image_encodings::BGR8, + // sensor_msgs::image_encodings::BGR8); + sensor_msgs::Image ros_image; + ros_image.header = msg->header; + ros_image.height = image.rows; + ros_image.width = image.cols; + ros_image.encoding = sensor_msgs::image_encodings::BGR8; + ros_image.is_bigendian = false; + ros_image.step = image.cols * image.elemSize(); + size_t size = ros_image.step * image.rows; + ros_image.data.resize(size); + + if (image.isContinuous()) + { + memcpy((char*)(&ros_image.data[0]), image.data, size); + } + else + { + // Copy by row by row + uchar* ros_data_ptr = (uchar*)(&ros_image.data[0]); + uchar* cv_data_ptr = image.data; + for (int i = 0; i < image.rows; ++i) + { + memcpy(ros_data_ptr, cv_data_ptr, ros_image.step); + ros_data_ptr += ros_image.step; + cv_data_ptr += image.step; + } + } + cv_ptr = cv_bridge::toCvCopy(ros_image, sensor_msgs::image_encodings::BGR8); #endif } catch (cv_bridge::Exception& e) @@ -146,15 +148,16 @@ class ImageConverter // Draw an example circle on the video stream if (cv_ptr->image.rows > 110 && cv_ptr->image.cols > 110) - cv::circle(cv_ptr->image, cv::Point(cv_ptr->image.cols/2, cv_ptr->image.rows/2), 100, CV_RGB(255,0,0)); + cv::circle(cv_ptr->image, cv::Point(cv_ptr->image.cols / 2, cv_ptr->image.rows / 2), 100, CV_RGB(255, 0, 0)); - if( debug_view_) { + if (debug_view_) + { // Update GUI Window cv::imshow(OPENCV_WINDOW, cv_ptr->image); cv::waitKey(3); } - // Output modified video stream +// Output modified video stream #ifndef CV_BRIDGE_COMPRESSED_IMAGE_IS_NOT_SUPPORTED image_pub_.publish(cv_ptr->toCompressedImageMsg()); #else @@ -168,48 +171,49 @@ class ImageConverter const std::string dst_format = std::string(); ros_image.header = cv_ptr->header; cv::Mat image; - if(cv_ptr->encoding != sensor_msgs::image_encodings::BGR8) - { - cv_bridge::CvImagePtr tempThis = boost::make_shared(*cv_ptr); - cv_bridge::CvImagePtr temp = cv_bridge::cvtColor(tempThis,sensor_msgs::image_encodings::BGR8); - cv_ptr->image = temp->image; - } + if (cv_ptr->encoding != sensor_msgs::image_encodings::BGR8) + { + cv_bridge::CvImagePtr tempThis = boost::make_shared(*cv_ptr); + cv_bridge::CvImagePtr temp = cv_bridge::cvtColor(tempThis, sensor_msgs::image_encodings::BGR8); + cv_ptr->image = temp->image; + } else - { - image = cv_ptr->image; - } + { + image = cv_ptr->image; + } std::vector buf; if (dst_format.empty() || dst_format == "jpg") - { - ros_image.format = "jpg"; - cv::imencode(".jpg", image, buf); - } + { + ros_image.format = "jpg"; + cv::imencode(".jpg", image, buf); + } if (dst_format == "png") - { - ros_image.format = "png"; - cv::imencode(".png", image, buf); - } + { + ros_image.format = "png"; + cv::imencode(".png", image, buf); + } - //TODO: check this formats (on rviz) and add more formats - //from http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags) + // TODO: check this formats (on rviz) and add more formats + // from http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const + // string& filename, int flags) if (dst_format == "jp2") - { - ros_image.format = "jp2"; - cv::imencode(".jp2", image, buf); - } + { + ros_image.format = "jp2"; + cv::imencode(".jp2", image, buf); + } if (dst_format == "bmp") - { - ros_image.format = "bmp"; - cv::imencode(".bmp", image, buf); - } + { + ros_image.format = "bmp"; + cv::imencode(".bmp", image, buf); + } if (dst_format == "tif") - { - ros_image.format = "tif"; - cv::imencode(".tif", image, buf); - } + { + ros_image.format = "tif"; + cv::imencode(".tif", image, buf); + } ros_image.data = buf; return ros_image; @@ -217,32 +221,33 @@ class ImageConverter #endif }; -} // namespace simple_compressed_example +} // namespace simple_compressed_example class SimpleCompressedExampleNodelet : public nodelet::Nodelet { - - public: virtual void onInit() { - simple_compressed_example::ImageConverter ic; - ros::spin(); + simple_compressed_example::ImageConverter ic; + ros::spin(); } }; -} // namespace opencv_apps +} // namespace opencv_apps -namespace simple_compressed_example { -class SimpleCompressedExampleNodelet : public opencv_apps::SimpleCompressedExampleNodelet { +namespace simple_compressed_example +{ +class SimpleCompressedExampleNodelet : public opencv_apps::SimpleCompressedExampleNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet simple_compressed_example/simple_compressed_example is deprecated, " "and renamed to opencv_apps/simple_compressed_example."); opencv_apps::SimpleCompressedExampleNodelet::onInit(); } }; -} // namespace simple_compressed_example +} // namespace simple_compressed_example #include PLUGINLIB_EXPORT_CLASS(opencv_apps::SimpleCompressedExampleNodelet, nodelet::Nodelet); diff --git a/src/nodelet/simple_example_nodelet.cpp b/src/nodelet/simple_example_nodelet.cpp index 4434ecdc..7389bfe6 100644 --- a/src/nodelet/simple_example_nodelet.cpp +++ b/src/nodelet/simple_example_nodelet.cpp @@ -3,11 +3,11 @@ * * Copyright (c) 2015, Tal Regev. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -//http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages +// http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages /** * This is a demo of Simple Example from wiki tutorial */ @@ -45,13 +45,12 @@ #include #include - #include -namespace opencv_apps { - -namespace simple_example { - +namespace opencv_apps +{ +namespace simple_example +{ static const std::string OPENCV_WINDOW = "Image window"; class ImageConverter @@ -64,25 +63,25 @@ class ImageConverter bool debug_view_; public: - ImageConverter() - : it_(nh_) + ImageConverter() : it_(nh_) { // Subscrive to input video feed and publish output video feed - image_sub_ = it_.subscribe("image", queue_size_, - &ImageConverter::imageCb, this); + image_sub_ = it_.subscribe("image", queue_size_, &ImageConverter::imageCb, this); image_pub_ = it_.advertise("/image_converter/output_video/raw", 1); ros::NodeHandle pnh_("~"); pnh_.param("queue_size", queue_size_, 1); pnh_.param("debug_view", debug_view_, false); - if( debug_view_) { + if (debug_view_) + { cv::namedWindow(OPENCV_WINDOW); } } ~ImageConverter() { - if( debug_view_) { + if (debug_view_) + { cv::destroyWindow(OPENCV_WINDOW); } } @@ -102,9 +101,10 @@ class ImageConverter // Draw an example circle on the video stream if (cv_ptr->image.rows > 110 && cv_ptr->image.cols > 110) - cv::circle(cv_ptr->image, cv::Point(cv_ptr->image.cols/2, cv_ptr->image.rows/2), 100, CV_RGB(255,0,0)); + cv::circle(cv_ptr->image, cv::Point(cv_ptr->image.cols / 2, cv_ptr->image.rows / 2), 100, CV_RGB(255, 0, 0)); - if( debug_view_) { + if (debug_view_) + { // Update GUI Window cv::imshow(OPENCV_WINDOW, cv_ptr->image); cv::waitKey(3); @@ -115,33 +115,33 @@ class ImageConverter } }; -} // namesapce simple_example +} // namesapce simple_example class SimpleExampleNodelet : public nodelet::Nodelet { - - public: virtual void onInit() { - simple_example::ImageConverter ic; - ros::spin(); + simple_example::ImageConverter ic; + ros::spin(); } }; -} // namespace opencv_apps +} // namespace opencv_apps -namespace simple_example { -class SimpleExampleNodelet : public opencv_apps::SimpleExampleNodelet { +namespace simple_example +{ +class SimpleExampleNodelet : public opencv_apps::SimpleExampleNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet simple_example/simple_example is deprecated, " "and renamed to opencv_apps/simple_example."); opencv_apps::SimpleExampleNodelet::onInit(); } }; -} // namespace simple_example - +} // namespace simple_example #include PLUGINLIB_EXPORT_CLASS(opencv_apps::SimpleExampleNodelet, nodelet::Nodelet); diff --git a/src/nodelet/simple_flow_nodelet.cpp b/src/nodelet/simple_flow_nodelet.cpp index bed52cf3..2512dd30 100644 --- a/src/nodelet/simple_flow_nodelet.cpp +++ b/src/nodelet/simple_flow_nodelet.cpp @@ -3,11 +3,11 @@ * * Copyright (c) 2014, Kei Okada. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -54,7 +54,8 @@ #include "opencv_apps/SimpleFlowConfig.h" #include "opencv_apps/FlowArrayStamped.h" -namespace opencv_apps { +namespace opencv_apps +{ class SimpleFlowNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -77,16 +78,16 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet std::string window_name_; static bool need_config_update_; int scale_; - + cv::Mat gray, prevGray; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; scale_ = config_.scale; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -97,13 +98,13 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet { do_work(msg, cam_info->header.frame_id); } - + void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -118,22 +119,28 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet /// Convert it to gray cv::Mat frame; - if ( frame_src.channels() > 1 ) { + if (frame_src.channels() > 1) + { frame = frame_src; - } else { - cv::cvtColor( frame_src, frame, cv::COLOR_GRAY2BGR ); + } + else + { + cv::cvtColor(frame_src, frame, cv::COLOR_GRAY2BGR); } - cv::resize(frame, gray, cv::Size(frame.cols/(double)MAX(1,scale_), frame.rows/(double)MAX(1,scale_)), 0, 0, CV_INTER_AREA); - if(prevGray.empty()) + cv::resize(frame, gray, cv::Size(frame.cols / (double)MAX(1, scale_), frame.rows / (double)MAX(1, scale_)), 0, 0, + CV_INTER_AREA); + if (prevGray.empty()) gray.copyTo(prevGray); - if (gray.rows != prevGray.rows && gray.cols != prevGray.cols) { + if (gray.rows != prevGray.rows && gray.cols != prevGray.cols) + { NODELET_WARN("Images should be of equal sizes"); gray.copyTo(prevGray); } - if (frame.type() != 16) { + if (frame.type() != 16) + { NODELET_ERROR("Images should be of equal type CV_8UC3"); } // Messages @@ -143,10 +150,12 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet // Do the work cv::Mat flow; - if( debug_view_) { - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); - cv::createTrackbar( "Scale", window_name_, &scale_, 24, trackbarCallback); - if (need_config_update_) { + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + cv::createTrackbar("Scale", window_name_, &scale_, 24, trackbarCallback); + if (need_config_update_) + { config_.scale = scale_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; @@ -159,42 +168,45 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet #else cv::calcOpticalFlowSF(gray, prevGray, #endif - flow, - 3, 2, 4, 4.1, 25.5, 18, 55.0, 25.5, 0.35, 18, 55.0, 25.5, 10); + flow, 3, 2, 4, 4.1, 25.5, 18, 55.0, 25.5, 0.35, 18, 55.0, 25.5, 10); NODELET_INFO("calcOpticalFlowSF : %lf sec", (cv::getTickCount() - start) / cv::getTickFrequency()); - //writeOpticalFlowToFile(flow, file); + // writeOpticalFlowToFile(flow, file); int cols = flow.cols; int rows = flow.rows; - double scale_col = frame.cols/(double)flow.cols; - double scale_row = frame.rows/(double)flow.rows; + double scale_col = frame.cols / (double)flow.cols; + double scale_row = frame.rows / (double)flow.rows; - - for (int i= 0; i < rows; ++i) { - for (int j = 0; j < cols; ++j) { + for (int i = 0; i < rows; ++i) + { + for (int j = 0; j < cols; ++j) + { cv::Vec2f flow_at_point = flow.at(i, j); - cv::line(frame, cv::Point(scale_col*j, scale_row*i), cv::Point(scale_col*(j+flow_at_point[0]), scale_row*(i+flow_at_point[1])), cv::Scalar(0,255,0), 1, 8, 0 ); + cv::line(frame, cv::Point(scale_col * j, scale_row * i), + cv::Point(scale_col * (j + flow_at_point[0]), scale_row * (i + flow_at_point[1])), + cv::Scalar(0, 255, 0), 1, 8, 0); opencv_apps::Flow flow_msg; opencv_apps::Point2D point_msg; opencv_apps::Point2D velocity_msg; - point_msg.x = scale_col*j; - point_msg.y = scale_row*i; - velocity_msg.x = scale_col*flow_at_point[0]; - velocity_msg.y = scale_row*flow_at_point[1]; + point_msg.x = scale_col * j; + point_msg.y = scale_row * i; + velocity_msg.x = scale_col * flow_at_point[0]; + velocity_msg.y = scale_row * flow_at_point[1]; flow_msg.point = point_msg; flow_msg.velocity = velocity_msg; flows_msg.flow.push_back(flow_msg); } } - //cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); + // cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); /// Apply Canny edge detector - //Canny( src_gray, edges, 50, 200, 3 ); + // Canny( src_gray, edges, 50, 200, 3 ); //-- Show what you got - if ( debug_view_) { - cv::imshow( window_name_, frame ); + if (debug_view_) + { + cv::imshow(window_name_, frame); int c = cv::waitKey(1); } @@ -204,7 +216,7 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet img_pub_.publish(out_img); msg_pub_.publish(flows_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -236,7 +248,8 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -246,9 +259,9 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&SimpleFlowNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&SimpleFlowNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); - + img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "flows", 1); @@ -256,18 +269,21 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet } }; bool SimpleFlowNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace simple_flow { -class SimpleFlowNodelet : public opencv_apps::SimpleFlowNodelet { +namespace simple_flow +{ +class SimpleFlowNodelet : public opencv_apps::SimpleFlowNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet simple_flow/simple_flow is deprecated, " "and renamed to opencv_apps/simple_flow."); opencv_apps::SimpleFlowNodelet::onInit(); } }; -} // namespace simple_flow +} // namespace simple_flow #include PLUGINLIB_EXPORT_CLASS(opencv_apps::SimpleFlowNodelet, nodelet::Nodelet); diff --git a/src/nodelet/smoothing_nodelet.cpp b/src/nodelet/smoothing_nodelet.cpp index f9ef7dd5..aab5e115 100644 --- a/src/nodelet/smoothing_nodelet.cpp +++ b/src/nodelet/smoothing_nodelet.cpp @@ -59,7 +59,8 @@ /// Global Variables int MAX_KERNEL_LENGTH = 31; -namespace opencv_apps { +namespace opencv_apps +{ class SmoothingNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -83,13 +84,13 @@ class SmoothingNodelet : public opencv_apps::Nodelet int kernel_size_; - void reconfigureCallback(Config &new_config, uint32_t level) + void reconfigureCallback(Config& new_config, uint32_t level) { - config_ = new_config; - kernel_size_ = (config_.kernel_size/2)*2+1; // kernel_size must be odd number + config_ = new_config; + kernel_size_ = (config_.kernel_size / 2) * 2 + 1; // kernel_size must be odd number } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -106,7 +107,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -119,15 +120,16 @@ class SmoothingNodelet : public opencv_apps::Nodelet // Convert the image into something opencv can handle. cv::Mat in_image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; - - if( debug_view_) { + if (debug_view_) + { /// Create Trackbars for Thresholds char kernel_label[] = "Kernel Size : "; - cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); - cv::createTrackbar( kernel_label, window_name_, &kernel_size_, MAX_KERNEL_LENGTH, trackbarCallback); - if (need_config_update_) { - kernel_size_ = (kernel_size_/2)*2+1; // kernel_size must be odd number + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + cv::createTrackbar(kernel_label, window_name_, &kernel_size_, MAX_KERNEL_LENGTH, trackbarCallback); + if (need_config_update_) + { + kernel_size_ = (kernel_size_ / 2) * 2 + 1; // kernel_size must be odd number config_.kernel_size = kernel_size_; reconfigure_server_->updateConfig(config_); need_config_update_ = false; @@ -136,40 +138,42 @@ class SmoothingNodelet : public opencv_apps::Nodelet cv::Mat out_image = in_image.clone(); int i = kernel_size_; - switch (config_.filter_type) { + switch (config_.filter_type) + { case opencv_apps::Smoothing_Homogeneous_Blur: - { - /// Applying Homogeneous blur - ROS_DEBUG_STREAM("Applying Homogeneous blur with kernel size " << i ); - cv::blur( in_image, out_image, cv::Size( i, i ), cv::Point(-1,-1) ); - break; - } + { + /// Applying Homogeneous blur + ROS_DEBUG_STREAM("Applying Homogeneous blur with kernel size " << i); + cv::blur(in_image, out_image, cv::Size(i, i), cv::Point(-1, -1)); + break; + } case opencv_apps::Smoothing_Gaussian_Blur: - { - /// Applying Gaussian blur - ROS_DEBUG_STREAM("Applying Gaussian blur with kernel size " << i ); - cv::GaussianBlur( in_image, out_image, cv::Size( i, i ), 0, 0 ); - break; - } + { + /// Applying Gaussian blur + ROS_DEBUG_STREAM("Applying Gaussian blur with kernel size " << i); + cv::GaussianBlur(in_image, out_image, cv::Size(i, i), 0, 0); + break; + } case opencv_apps::Smoothing_Median_Blur: - { - /// Applying Median blur - ROS_DEBUG_STREAM("Applying Median blur with kernel size " << i ); - cv::medianBlur ( in_image, out_image, i ); - break; - } + { + /// Applying Median blur + ROS_DEBUG_STREAM("Applying Median blur with kernel size " << i); + cv::medianBlur(in_image, out_image, i); + break; + } case opencv_apps::Smoothing_Bilateral_Filter: - { - /// Applying Bilateral Filter - ROS_DEBUG_STREAM("Applying Bilateral blur with kernel size " << i ); - cv::bilateralFilter ( in_image, out_image, i, i*2, i/2 ); - break; - } + { + /// Applying Bilateral Filter + ROS_DEBUG_STREAM("Applying Bilateral blur with kernel size " << i); + cv::bilateralFilter(in_image, out_image, i, i * 2, i / 2); + break; + } } //-- Show what you got - if( debug_view_) { - cv::imshow( window_name_, out_image ); + if (debug_view_) + { + cv::imshow(window_name_, out_image); int c = cv::waitKey(1); } @@ -177,7 +181,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).toImageMsg(); img_pub_.publish(out_img); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -209,7 +213,8 @@ class SmoothingNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -219,7 +224,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&SmoothingNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&SmoothingNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); @@ -228,18 +233,21 @@ class SmoothingNodelet : public opencv_apps::Nodelet } }; bool SmoothingNodelet::need_config_update_ = false; -} // namespace opencv_apps +} // namespace opencv_apps -namespace smoothing { -class SmoothingNodelet : public opencv_apps::SmoothingNodelet { +namespace smoothing +{ +class SmoothingNodelet : public opencv_apps::SmoothingNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet smoothing/smoothing is deprecated, " "and renamed to opencv_apps/smoothing."); opencv_apps::SmoothingNodelet::onInit(); } }; -} // namespace smoothing +} // namespace smoothing #include PLUGINLIB_EXPORT_CLASS(opencv_apps::SmoothingNodelet, nodelet::Nodelet); diff --git a/src/nodelet/threshold_nodelet.cpp b/src/nodelet/threshold_nodelet.cpp index b400c001..32416145 100644 --- a/src/nodelet/threshold_nodelet.cpp +++ b/src/nodelet/threshold_nodelet.cpp @@ -51,135 +51,140 @@ #include -namespace opencv_apps { - class ThresholdNodelet : public opencv_apps::Nodelet { - //////////////////////////////////////////////////////// - // Dynamic Reconfigure - //////////////////////////////////////////////////////// - typedef opencv_apps::ThresholdConfig Config; - typedef dynamic_reconfigure::Server ReconfigureServer; - Config config_; - boost::shared_ptr reconfigure_server_; - - int queue_size_; - bool debug_view_; - - std::string window_name_; - - image_transport::Publisher img_pub_; - image_transport::Subscriber img_sub_; - image_transport::CameraSubscriber cam_sub_; - - boost::shared_ptr it_; - - boost::mutex mutex_; - int threshold_type_; - int max_binary_value_; - int threshold_value_; - bool apply_otsu_; - - void imageCallbackWithInfo( - const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); - } +namespace opencv_apps +{ +class ThresholdNodelet : public opencv_apps::Nodelet +{ + //////////////////////////////////////////////////////// + // Dynamic Reconfigure + //////////////////////////////////////////////////////// + typedef opencv_apps::ThresholdConfig Config; + typedef dynamic_reconfigure::Server ReconfigureServer; + Config config_; + boost::shared_ptr reconfigure_server_; + + int queue_size_; + bool debug_view_; + + std::string window_name_; + + image_transport::Publisher img_pub_; + image_transport::Subscriber img_sub_; + image_transport::CameraSubscriber cam_sub_; + + boost::shared_ptr it_; + + boost::mutex mutex_; + int threshold_type_; + int max_binary_value_; + int threshold_value_; + bool apply_otsu_; + + void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) + { + do_work(msg, cam_info->header.frame_id); + } - void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); - } + void imageCallback(const sensor_msgs::ImageConstPtr& msg) + { + do_work(msg, msg->header.frame_id); + } - void subscribe() { - NODELET_DEBUG("Subscribing to image topic."); - if (config_.use_camera_info) - cam_sub_ = it_->subscribeCamera( - "image", queue_size_, &ThresholdNodelet::imageCallbackWithInfo, this); - else - img_sub_ = - it_->subscribe("image", queue_size_, &ThresholdNodelet::imageCallback, this); - } + void subscribe() + { + NODELET_DEBUG("Subscribing to image topic."); + if (config_.use_camera_info) + cam_sub_ = it_->subscribeCamera("image", queue_size_, &ThresholdNodelet::imageCallbackWithInfo, this); + else + img_sub_ = it_->subscribe("image", queue_size_, &ThresholdNodelet::imageCallback, this); + } - void unsubscribe() { - NODELET_DEBUG("Unsubscribing from image topic."); - img_sub_.shutdown(); - cam_sub_.shutdown(); - } + void unsubscribe() + { + NODELET_DEBUG("Unsubscribing from image topic."); + img_sub_.shutdown(); + cam_sub_.shutdown(); + } - void reconfigureCallback(Config& config, uint32_t level) { - boost::mutex::scoped_lock lock(mutex_); - config_ = config; - threshold_value_ = config.threshold; - threshold_type_ = config.threshold_type; - max_binary_value_ = config.max_binary; - apply_otsu_ = config.apply_otsu; - } + void reconfigureCallback(Config& config, uint32_t level) + { + boost::mutex::scoped_lock lock(mutex_); + config_ = config; + threshold_value_ = config.threshold; + threshold_type_ = config.threshold_type; + max_binary_value_ = config.max_binary; + apply_otsu_ = config.apply_otsu; + } - void do_work(const sensor_msgs::Image::ConstPtr& image_msg, - const std::string input_frame_from_msg) { - try { - cv::Mat src_image = - cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8)->image; - cv::Mat gray_image; - cv::cvtColor(src_image, gray_image, cv::COLOR_BGR2GRAY); - cv::Mat result_image; - - if (apply_otsu_) { - threshold_type_ |= CV_THRESH_OTSU; - } - cv::threshold(gray_image, result_image, threshold_value_, - max_binary_value_, threshold_type_); - //-- Show what you got - if (debug_view_) { - cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); - cv::imshow(window_name_, result_image); - int c = cv::waitKey(1); - } - img_pub_.publish(cv_bridge::CvImage(image_msg->header, - sensor_msgs::image_encodings::MONO8, - result_image).toImageMsg()); + void do_work(const sensor_msgs::Image::ConstPtr& image_msg, const std::string input_frame_from_msg) + { + try + { + cv::Mat src_image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8)->image; + cv::Mat gray_image; + cv::cvtColor(src_image, gray_image, cv::COLOR_BGR2GRAY); + cv::Mat result_image; + + if (apply_otsu_) + { + threshold_type_ |= CV_THRESH_OTSU; } - catch (cv::Exception& e) { - NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), - e.func.c_str(), e.file.c_str(), e.line); + cv::threshold(gray_image, result_image, threshold_value_, max_binary_value_, threshold_type_); + //-- Show what you got + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + cv::imshow(window_name_, result_image); + int c = cv::waitKey(1); } + img_pub_.publish( + cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::MONO8, result_image).toImageMsg()); } + catch (cv::Exception& e) + { + NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); + } + } - public: - virtual void onInit() { - Nodelet::onInit(); - it_ = boost::shared_ptr( - new image_transport::ImageTransport(*nh_)); - - pnh_->param("queue_size", queue_size_, 3); - pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { - always_subscribe_ = true; - } - //////////////////////////////////////////////////////// - // Dynamic Reconfigure - //////////////////////////////////////////////////////// - reconfigure_server_ = - boost::make_shared >(*pnh_); - dynamic_reconfigure::Server::CallbackType f = +public: + virtual void onInit() + { + Nodelet::onInit(); + it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + + pnh_->param("queue_size", queue_size_, 3); + pnh_->param("debug_view", debug_view_, false); + if (debug_view_) + { + always_subscribe_ = true; + } + //////////////////////////////////////////////////////// + // Dynamic Reconfigure + //////////////////////////////////////////////////////// + reconfigure_server_ = boost::make_shared >(*pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind(&ThresholdNodelet::reconfigureCallback, this, _1, _2); - reconfigure_server_->setCallback(f); + reconfigure_server_->setCallback(f); - img_pub_ = advertiseImage(*pnh_, "image", 1); - onInitPostProcess(); - } - }; -} // namespace opencv_apps + img_pub_ = advertiseImage(*pnh_, "image", 1); + onInitPostProcess(); + } +}; +} // namespace opencv_apps -namespace threshold { -class ThresholdNodelet : public opencv_apps::ThresholdNodelet { +namespace threshold +{ +class ThresholdNodelet : public opencv_apps::ThresholdNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet threshold/threshold is deprecated, " "and renamed to opencv_apps/threshold."); opencv_apps::ThresholdNodelet::onInit(); } }; -} // namespace threshold - +} // namespace threshold #include PLUGINLIB_EXPORT_CLASS(opencv_apps::ThresholdNodelet, nodelet::Nodelet); diff --git a/src/nodelet/watershed_segmentation_nodelet.cpp b/src/nodelet/watershed_segmentation_nodelet.cpp index 3b542f76..c16a8a71 100644 --- a/src/nodelet/watershed_segmentation_nodelet.cpp +++ b/src/nodelet/watershed_segmentation_nodelet.cpp @@ -3,11 +3,11 @@ * * Copyright (c) 2014, Kei Okada. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Kei Okada nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -53,7 +53,8 @@ #include "opencv_apps/ContourArrayStamped.h" #include "opencv_apps/Point2DArray.h" -namespace opencv_apps { +namespace opencv_apps +{ class WatershedSegmentationNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; @@ -84,7 +85,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet cv::Mat markerMask; cv::Point prevPt; - static void onMouse( int event, int x, int y, int flags, void* ) + static void onMouse(int event, int x, int y, int flags, void*) { on_mouse_update_ = true; on_mouse_event_ = event; @@ -93,12 +94,12 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet on_mouse_flags_ = flags; } - void reconfigureCallback(opencv_apps::WatershedSegmentationConfig &new_config, uint32_t level) + void reconfigureCallback(opencv_apps::WatershedSegmentationConfig& new_config, uint32_t level) { config_ = new_config; } - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) { if (frame.empty()) return image_frame; @@ -109,13 +110,13 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet { do_work(msg, cam_info->header.frame_id); } - + void imageCallback(const sensor_msgs::ImageConstPtr& msg) { do_work(msg, msg->header.frame_id); } - static void trackbarCallback( int, void* ) + static void trackbarCallback(int, void*) { need_config_update_ = true; } @@ -133,73 +134,81 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet contours_msg.header = msg->header; // Do the work - //std::vector faces; + // std::vector faces; cv::Mat imgGray; /// Initialize - if ( markerMask.empty() ) { + if (markerMask.empty()) + { cv::cvtColor(frame, markerMask, cv::COLOR_BGR2GRAY); cv::cvtColor(markerMask, imgGray, cv::COLOR_GRAY2BGR); markerMask = cv::Scalar::all(0); } - if( debug_view_) { - cv::imshow( window_name_, frame); - cv::setMouseCallback( window_name_, onMouse, 0 ); - if (need_config_update_) { + if (debug_view_) + { + cv::imshow(window_name_, frame); + cv::setMouseCallback(window_name_, onMouse, 0); + if (need_config_update_) + { reconfigure_server_->updateConfig(config_); need_config_update_ = false; } - if ( on_mouse_update_ ) { + if (on_mouse_update_) + { int event = on_mouse_event_; int x = on_mouse_x_; int y = on_mouse_y_; int flags = on_mouse_flags_; - if( x < 0 || x >= frame.cols || y < 0 || y >= frame.rows ) + if (x < 0 || x >= frame.cols || y < 0 || y >= frame.rows) return; - if( event == cv::EVENT_LBUTTONUP || !(flags & cv::EVENT_FLAG_LBUTTON) ) - prevPt = cv::Point(-1,-1); - else if( event == cv::EVENT_LBUTTONDOWN ) - prevPt = cv::Point(x,y); - else if( event == cv::EVENT_MOUSEMOVE && (flags & cv::EVENT_FLAG_LBUTTON) ) + if (event == cv::EVENT_LBUTTONUP || !(flags & cv::EVENT_FLAG_LBUTTON)) + prevPt = cv::Point(-1, -1); + else if (event == cv::EVENT_LBUTTONDOWN) + prevPt = cv::Point(x, y); + else if (event == cv::EVENT_MOUSEMOVE && (flags & cv::EVENT_FLAG_LBUTTON)) { cv::Point pt(x, y); - if( prevPt.x < 0 ) + if (prevPt.x < 0) prevPt = pt; - cv::line( markerMask, prevPt, pt, cv::Scalar::all(255), 5, 8, 0 ); - cv::line( frame, prevPt, pt, cv::Scalar::all(255), 5, 8, 0 ); + cv::line(markerMask, prevPt, pt, cv::Scalar::all(255), 5, 8, 0); + cv::line(frame, prevPt, pt, cv::Scalar::all(255), 5, 8, 0); prevPt = pt; cv::imshow(window_name_, markerMask); } } cv::waitKey(1); } - + int i, j, compCount = 0; std::vector > contours; std::vector hierarchy; cv::findContours(markerMask, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE); - if( contours.empty() ) { + if (contours.empty()) + { NODELET_WARN("contours are empty"); - return; //continue; + return; // continue; } cv::Mat markers(markerMask.size(), CV_32S); markers = cv::Scalar::all(0); int idx = 0; - for( ; idx >= 0; idx = hierarchy[idx][0], compCount++ ) - cv::drawContours(markers, contours, idx, cv::Scalar::all(compCount+1), -1, 8, hierarchy, INT_MAX); + for (; idx >= 0; idx = hierarchy[idx][0], compCount++) + cv::drawContours(markers, contours, idx, cv::Scalar::all(compCount + 1), -1, 8, hierarchy, INT_MAX); - if( compCount == 0 ) { + if (compCount == 0) + { NODELET_WARN("compCount is 0"); - return; //continue; + return; // continue; } - for( size_t i = 0; i< contours.size(); i++ ) { + for (size_t i = 0; i < contours.size(); i++) + { opencv_apps::Contour contour_msg; - for ( size_t j = 0; j < contours[i].size(); j++ ) { + for (size_t j = 0; j < contours[i].size(); j++) + { opencv_apps::Point2D point_msg; point_msg.x = contours[i][j].x; point_msg.y = contours[i][j].y; @@ -209,7 +218,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet } std::vector colorTab; - for( i = 0; i < compCount; i++ ) + for (i = 0; i < compCount; i++) { int b = cv::theRNG().uniform(0, 255); int g = cv::theRNG().uniform(0, 255); @@ -219,35 +228,36 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet } double t = (double)cv::getTickCount(); - cv::watershed( frame, markers ); + cv::watershed(frame, markers); t = (double)cv::getTickCount() - t; - NODELET_INFO( "execution time = %gms", t*1000./cv::getTickFrequency() ); + NODELET_INFO("execution time = %gms", t * 1000. / cv::getTickFrequency()); cv::Mat wshed(markers.size(), CV_8UC3); // paint the watershed image - for( i = 0; i < markers.rows; i++ ) - for( j = 0; j < markers.cols; j++ ) + for (i = 0; i < markers.rows; i++) + for (j = 0; j < markers.cols; j++) { - int index = markers.at(i,j); - if( index == -1 ) - wshed.at(i,j) = cv::Vec3b(255,255,255); - else if( index <= 0 || index > compCount ) - wshed.at(i,j) = cv::Vec3b(0,0,0); + int index = markers.at(i, j); + if (index == -1) + wshed.at(i, j) = cv::Vec3b(255, 255, 255); + else if (index <= 0 || index > compCount) + wshed.at(i, j) = cv::Vec3b(0, 0, 0); else - wshed.at(i,j) = colorTab[index - 1]; + wshed.at(i, j) = colorTab[index - 1]; } - wshed = wshed*0.5 + imgGray*0.5; + wshed = wshed * 0.5 + imgGray * 0.5; //-- Show what you got - if( debug_view_) { - cv::imshow( segment_name_, wshed ); + if (debug_view_) + { + cv::imshow(segment_name_, wshed); int c = cv::waitKey(1); - //if( (char)c == 27 ) + // if( (char)c == 27 ) // break; - if( (char)c == 'r' ) + if ((char)c == 'r') { markerMask = cv::Scalar::all(0); } @@ -258,7 +268,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet img_pub_.publish(out_img); msg_pub_.publish(contours_msg); } - catch (cv::Exception &e) + catch (cv::Exception& e) { NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -266,14 +276,19 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void add_seed_point_cb(const opencv_apps::Point2DArray& msg) { - if ( msg.points.size() == 0 ) { + void add_seed_point_cb(const opencv_apps::Point2DArray& msg) + { + if (msg.points.size() == 0) + { markerMask = cv::Scalar::all(0); - } else { - for(size_t i = 0; i < msg.points.size(); i++ ) { + } + else + { + for (size_t i = 0; i < msg.points.size(); i++) + { cv::Point pt0(msg.points[i].x, msg.points[i].y); cv::Point pt1(pt0.x + 1, pt0.y + 1); - cv::line( markerMask, pt0, pt1, cv::Scalar::all(255), 5, 8, 0 ); + cv::line(markerMask, pt0, pt1, cv::Scalar::all(255), 5, 8, 0); } } } @@ -302,7 +317,8 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet pnh_->param("queue_size", queue_size_, 3); pnh_->param("debug_view", debug_view_, false); - if (debug_view_) { + if (debug_view_) + { always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -314,13 +330,13 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&WatershedSegmentationNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&WatershedSegmentationNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); - add_seed_points_sub_ = pnh_->subscribe("add_seed_points", 1, &WatershedSegmentationNodelet::add_seed_point_cb, this); + add_seed_points_sub_ = + pnh_->subscribe("add_seed_points", 1, &WatershedSegmentationNodelet::add_seed_point_cb, this); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "contours", 1); - NODELET_INFO("This program demonstrates the famous watershed segmentation algorithm in OpenCV: watershed()"); NODELET_INFO("Hot keys: "); @@ -339,18 +355,21 @@ int WatershedSegmentationNodelet::on_mouse_event_ = 0; int WatershedSegmentationNodelet::on_mouse_x_ = 0; int WatershedSegmentationNodelet::on_mouse_y_ = 0; int WatershedSegmentationNodelet::on_mouse_flags_ = 0; -} // namespace opencv_apps +} // namespace opencv_apps -namespace watershed_segmentation { -class WatershedSegmentationNodelet : public opencv_apps::WatershedSegmentationNodelet { +namespace watershed_segmentation +{ +class WatershedSegmentationNodelet : public opencv_apps::WatershedSegmentationNodelet +{ public: - virtual void onInit() { + virtual void onInit() + { ROS_WARN("DeprecationWarning: Nodelet watershed_segmentation/watershed_segmentation is deprecated, " "and renamed to opencv_apps/watershed_segmentation."); opencv_apps::WatershedSegmentationNodelet::onInit(); } }; -} // namespace watershed_segmentation +} // namespace watershed_segmentation #include PLUGINLIB_EXPORT_CLASS(opencv_apps::WatershedSegmentationNodelet, nodelet::Nodelet); From 4c4caef9a812534cfd2426b6fed30b0a33f4d3d3 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sun, 21 Apr 2019 14:12:20 +0000 Subject: [PATCH 4/6] fix code by run-clang-tidy -fix --- include/opencv_apps/nodelet.h | 5 ++ src/nodelet/adding_images_nodelet.cpp | 28 ++++----- src/nodelet/camshift_nodelet.cpp | 52 +++++++-------- src/nodelet/color_filter_nodelet.cpp | 36 +++++------ src/nodelet/contour_moments_nodelet.cpp | 18 +++--- src/nodelet/convex_hull_nodelet.cpp | 22 +++---- src/nodelet/corner_harris_nodelet.cpp | 22 +++---- .../discrete_fourier_transform_nodelet.cpp | 14 ++--- src/nodelet/edge_detection_nodelet.cpp | 18 +++--- src/nodelet/face_detection_nodelet.cpp | 52 +++++++-------- src/nodelet/face_recognition_nodelet.cpp | 26 ++++---- src/nodelet/fback_flow_nodelet.cpp | 16 ++--- src/nodelet/find_contours_nodelet.cpp | 22 +++---- src/nodelet/general_contours_nodelet.cpp | 48 +++++++------- src/nodelet/goodfeature_track_nodelet.cpp | 44 ++++++------- src/nodelet/hough_circles_nodelet.cpp | 20 +++--- src/nodelet/hough_lines_nodelet.cpp | 23 ++++--- src/nodelet/lk_flow_nodelet.cpp | 38 ++++++----- src/nodelet/nodelet.cpp | 11 ++-- src/nodelet/people_detect_nodelet.cpp | 16 ++--- src/nodelet/phase_corr_nodelet.cpp | 16 ++--- src/nodelet/pyramids_nodelet.cpp | 14 ++--- src/nodelet/segment_objects_nodelet.cpp | 45 +++++++------ .../simple_compressed_example_nodelet.cpp | 10 +-- src/nodelet/simple_example_nodelet.cpp | 12 ++-- src/nodelet/simple_flow_nodelet.cpp | 16 ++--- src/nodelet/smoothing_nodelet.cpp | 16 ++--- src/nodelet/threshold_nodelet.cpp | 14 ++--- .../watershed_segmentation_nodelet.cpp | 63 +++++++++---------- 29 files changed, 367 insertions(+), 370 deletions(-) diff --git a/include/opencv_apps/nodelet.h b/include/opencv_apps/nodelet.h index f731eb14..d23e09f9 100644 --- a/include/opencv_apps/nodelet.h +++ b/include/opencv_apps/nodelet.h @@ -40,6 +40,11 @@ #include #include +// https://stackoverflow.com/questions/10496824/how-to-define-nullptr-for-supporting-both-c03-and-c11 +#if !defined(nullptr) +#define nullptr NULL +#endif + namespace opencv_apps { /** @brief diff --git a/src/nodelet/adding_images_nodelet.cpp b/src/nodelet/adding_images_nodelet.cpp index f4d5cd0e..18d2e600 100644 --- a/src/nodelet/adding_images_nodelet.cpp +++ b/src/nodelet/adding_images_nodelet.cpp @@ -101,15 +101,15 @@ class AddingImagesNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg1, const sensor_msgs::ImageConstPtr& msg2, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg1, msg2, cam_info->header.frame_id); + doWork(msg1, msg2, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg1, const sensor_msgs::ImageConstPtr& msg2) { - do_work(msg1, msg2, msg1->header.frame_id); + doWork(msg1, msg2, msg1->header.frame_id); } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); sub_image1_.subscribe(*it_, "image1", 3); @@ -148,7 +148,7 @@ class AddingImagesNodelet : public opencv_apps::Nodelet } } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); sub_image1_.unsubscribe(); @@ -173,8 +173,8 @@ class AddingImagesNodelet : public opencv_apps::Nodelet gamma_ = config.gamma; } - void do_work(const sensor_msgs::Image::ConstPtr& image_msg1, const sensor_msgs::Image::ConstPtr& image_msg2, - const std::string input_frame_from_msg) + void doWork(const sensor_msgs::Image::ConstPtr& image_msg1, const sensor_msgs::Image::ConstPtr& image_msg2, + const std::string& input_frame_from_msg) { boost::mutex::scoped_lock lock(mutex_); // Work on the image. @@ -195,14 +195,14 @@ class AddingImagesNodelet : public opencv_apps::Nodelet int new_rows = std::max(image1.rows, image2.rows); int new_cols = std::max(image1.cols, image2.cols); // if ( new_rows != image1.rows || new_cols != image1.cols ) { - cv::Mat image1_ = cv::Mat(new_rows, new_cols, image1.type()); - image1.copyTo(image1_(cv::Rect(0, 0, image1.cols, image1.rows))); - image1 = image1_.clone(); // need clone becuase toCvShare?? + cv::Mat image1 = cv::Mat(new_rows, new_cols, image1.type()); + image1.copyTo(image1(cv::Rect(0, 0, image1.cols, image1.rows))); + image1 = image1.clone(); // need clone becuase toCvShare?? // if ( new_rows != image2.rows || new_cols != image2.cols ) { - cv::Mat image2_ = cv::Mat(new_rows, new_cols, image2.type()); - image2.copyTo(image2_(cv::Rect(0, 0, image2.cols, image2.rows))); - image2 = image2_.clone(); + cv::Mat image2 = cv::Mat(new_rows, new_cols, image2.type()); + image2.copyTo(image2(cv::Rect(0, 0, image2.cols, image2.rows))); + image2 = image2.clone(); } cv::addWeighted(image1, alpha_, image2, beta_, gamma_, result_image); //-- Show what you got @@ -241,7 +241,7 @@ class AddingImagesNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -275,7 +275,7 @@ namespace adding_images class AddingImagesNodelet : public opencv_apps::AddingImagesNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet adding_images/adding_images is deprecated, " "and renamed to opencv_apps/adding_images."); diff --git a/src/nodelet/camshift_nodelet.cpp b/src/nodelet/camshift_nodelet.cpp index 512fef50..e3210c31 100644 --- a/src/nodelet/camshift_nodelet.cpp +++ b/src/nodelet/camshift_nodelet.cpp @@ -99,7 +99,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet cv::Mat hist, histimg; // cv::Mat hsv; - static void onMouse(int event, int x, int y, int, void*) + static void onMouse(int event, int x, int y, int /*unused*/, void* /*unused*/) { on_mouse_update_ = true; on_mouse_event_ = event; @@ -124,20 +124,20 @@ class CamShiftNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -158,7 +158,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); - cv::setMouseCallback(window_name_, onMouse, 0); + cv::setMouseCallback(window_name_, onMouse, nullptr); cv::createTrackbar("Vmin", window_name_, &vmin_, 256, trackbarCallback); cv::createTrackbar("Vmax", window_name_, &vmax_, 256, trackbarCallback); cv::createTrackbar("Smin", window_name_, &smin_, 256, trackbarCallback); @@ -212,9 +212,9 @@ class CamShiftNodelet : public opencv_apps::Nodelet if (trackObject) { - int _vmin = vmin_, _vmax = vmax_; + int vmin = vmin_, vmax = vmax_; - cv::inRange(hsv, cv::Scalar(0, smin_, MIN(_vmin, _vmax)), cv::Scalar(180, 256, MAX(_vmin, _vmax)), mask); + cv::inRange(hsv, cv::Scalar(0, smin_, MIN(vmin, vmax)), cv::Scalar(180, 256, MAX(vmin, vmax)), mask); int ch[] = { 0, 0 }; hue.create(hsv.size(), hsv.depth()); cv::mixChannels(&hsv, 1, &hue, 1, ch, 1); @@ -222,7 +222,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet if (trackObject < 0) { cv::Mat roi(hue, selection), maskroi(mask, selection); - cv::calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges); + cv::calcHist(&roi, 1, nullptr, maskroi, hist, 1, &hsize, &phranges); cv::normalize(hist, hist, 0, 255, cv::NORM_MINMAX); std::vector hist_value; hist_value.resize(hsize); @@ -236,7 +236,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet trackObject = 1; histimg = cv::Scalar::all(0); - int binW = histimg.cols / hsize; + int bin_w = histimg.cols / hsize; cv::Mat buf(1, hsize, CV_8UC3); for (int i = 0; i < hsize; i++) buf.at(i) = cv::Vec3b(cv::saturate_cast(i * 180. / hsize), 255, 255); @@ -245,14 +245,14 @@ class CamShiftNodelet : public opencv_apps::Nodelet for (int i = 0; i < hsize; i++) { int val = cv::saturate_cast(hist.at(i) * histimg.rows / 255); - cv::rectangle(histimg, cv::Point(i * binW, histimg.rows), cv::Point((i + 1) * binW, histimg.rows - val), + cv::rectangle(histimg, cv::Point(i * bin_w, histimg.rows), cv::Point((i + 1) * bin_w, histimg.rows - val), cv::Scalar(buf.at(i)), -1, 8); } } - cv::calcBackProject(&hue, 1, 0, hist, backproj, &phranges); + cv::calcBackProject(&hue, 1, nullptr, hist, backproj, &phranges); backproj &= mask; - cv::RotatedRect trackBox = cv::CamShift( + cv::RotatedRect track_box = cv::CamShift( backproj, trackWindow, cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 10, 1)); if (trackWindow.area() <= 1) { @@ -265,18 +265,18 @@ class CamShiftNodelet : public opencv_apps::Nodelet if (backprojMode) cv::cvtColor(backproj, frame, cv::COLOR_GRAY2BGR); #ifndef CV_VERSION_EPOCH - cv::ellipse(frame, trackBox, cv::Scalar(0, 0, 255), 3, cv::LINE_AA); + cv::ellipse(frame, track_box, cv::Scalar(0, 0, 255), 3, cv::LINE_AA); #else - cv::ellipse(frame, trackBox, cv::Scalar(0, 0, 255), 3, CV_AA); + cv::ellipse(frame, track_box, cv::Scalar(0, 0, 255), 3, CV_AA); #endif - rect_msg.rect.angle = trackBox.angle; + rect_msg.rect.angle = track_box.angle; opencv_apps::Point2D point_msg; opencv_apps::Size size_msg; - point_msg.x = trackBox.center.x; - point_msg.y = trackBox.center.y; - size_msg.width = trackBox.size.width; - size_msg.height = trackBox.size.height; + point_msg.x = track_box.center.x; + point_msg.y = track_box.center.y; + size_msg.width = track_box.size.width; + size_msg.height = track_box.size.height; rect_msg.rect.center = point_msg; rect_msg.rect.size = size_msg; } @@ -338,7 +338,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -347,7 +347,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &CamShiftNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -355,7 +355,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -416,7 +416,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet trackWindow = cv::Rect(0, 0, 640, 480); // histimg = cv::Scalar::all(0); - int binW = histimg.cols / hsize; + int bin_w = histimg.cols / hsize; cv::Mat buf(1, hsize, CV_8UC3); for (int i = 0; i < hsize; i++) buf.at(i) = cv::Vec3b(cv::saturate_cast(i * 180. / hsize), 255, 255); @@ -425,7 +425,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet for (int i = 0; i < hsize; i++) { int val = cv::saturate_cast(hist.at(i) * histimg.rows / 255); - cv::rectangle(histimg, cv::Point(i * binW, histimg.rows), cv::Point((i + 1) * binW, histimg.rows - val), + cv::rectangle(histimg, cv::Point(i * bin_w, histimg.rows), cv::Point((i + 1) * bin_w, histimg.rows - val), cv::Scalar(buf.at(i)), -1, 8); } } @@ -444,7 +444,7 @@ namespace camshift class CamShiftNodelet : public opencv_apps::CamShiftNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet camshift/camshift is deprecated, " "and renamed to opencv_apps/camshift."); diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp index 1f796ac0..2939b4d4 100644 --- a/src/nodelet/color_filter_nodelet.cpp +++ b/src/nodelet/color_filter_nodelet.cpp @@ -99,15 +99,15 @@ class ColorFilterNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - void do_work(const sensor_msgs::ImageConstPtr& image_msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& image_msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -148,7 +148,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -157,7 +157,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &ColorFilterNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -165,7 +165,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -201,7 +201,7 @@ class RGBColorFilterNodelet : public ColorFilterNodelet contour1, std::vector contour2) +bool compareContourAreas(const std::vector& contour1, const std::vector& contour2) { double i = fabs(contourArea(cv::Mat(contour1))); double j = fabs(contourArea(cv::Mat(contour2))); @@ -103,20 +103,20 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -249,7 +249,7 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -258,7 +258,7 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &ContourMomentsNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -266,7 +266,7 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -300,7 +300,7 @@ namespace contour_moments class ContourMomentsNodelet : public opencv_apps::ContourMomentsNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet contour_moments/contour_moments is deprecated, " "and renamed to opencv_apps/contour_moments."); diff --git a/src/nodelet/convex_hull_nodelet.cpp b/src/nodelet/convex_hull_nodelet.cpp index 4e8dd659..e2fb95d4 100644 --- a/src/nodelet/convex_hull_nodelet.cpp +++ b/src/nodelet/convex_hull_nodelet.cpp @@ -95,20 +95,20 @@ class ConvexHullNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -168,11 +168,11 @@ class ConvexHullNodelet : public opencv_apps::Nodelet cv::drawContours(drawing, hull, (int)i, color, 4, 8, std::vector(), 0, cv::Point()); opencv_apps::Contour contour_msg; - for (size_t j = 0; j < hull[i].size(); j++) + for (const cv::Point& j : hull[i]) { opencv_apps::Point2D point_msg; - point_msg.x = hull[i][j].x; - point_msg.y = hull[i][j].y; + point_msg.x = j.x; + point_msg.y = j.y; contour_msg.points.push_back(point_msg); } contours_msg.contours.push_back(contour_msg); @@ -207,7 +207,7 @@ class ConvexHullNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -216,7 +216,7 @@ class ConvexHullNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &ConvexHullNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -224,7 +224,7 @@ class ConvexHullNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -258,7 +258,7 @@ namespace convex_hull class ConvexHullNodelet : public opencv_apps::ConvexHullNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet convex_hull/convex_hull is deprecated, " "and renamed to opencv_apps/convex_hull."); diff --git a/src/nodelet/corner_harris_nodelet.cpp b/src/nodelet/corner_harris_nodelet.cpp index eec4b3be..5b9bbde7 100644 --- a/src/nodelet/corner_harris_nodelet.cpp +++ b/src/nodelet/corner_harris_nodelet.cpp @@ -90,20 +90,20 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& image_msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& image_msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -128,12 +128,12 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet } /// Detector parameters - int blockSize = 2; - int apertureSize = 3; + int block_size = 2; + int aperture_size = 3; double k = 0.04; /// Detecting corners - cv::cornerHarris(src_gray, dst, blockSize, apertureSize, k, cv::BORDER_DEFAULT); + cv::cornerHarris(src_gray, dst, block_size, aperture_size, k, cv::BORDER_DEFAULT); /// Normalizing cv::normalize(dst, dst_norm, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat()); @@ -182,7 +182,7 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet } } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -191,7 +191,7 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &CornerHarrisNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -199,7 +199,7 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -232,7 +232,7 @@ namespace corner_harris class CornerHarrisNodelet : public opencv_apps::CornerHarrisNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet corner_harris/corner_harris is deprecated, " "and renamed to opencv_apps/corner_harris."); diff --git a/src/nodelet/discrete_fourier_transform_nodelet.cpp b/src/nodelet/discrete_fourier_transform_nodelet.cpp index 0ca2ad07..da46d04e 100644 --- a/src/nodelet/discrete_fourier_transform_nodelet.cpp +++ b/src/nodelet/discrete_fourier_transform_nodelet.cpp @@ -75,15 +75,15 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -93,7 +93,7 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &DiscreteFourierTransformNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -106,7 +106,7 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet config_ = config; } - void do_work(const sensor_msgs::Image::ConstPtr& image_msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::Image::ConstPtr& image_msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -185,7 +185,7 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -216,7 +216,7 @@ namespace discrete_fourier_transform class DiscreteFourierTransformNodelet : public opencv_apps::DiscreteFourierTransformNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet discrete_fourier_transform/discrete_fourier_transform is deprecated, " "and renamed to opencv_apps/discrete_fourier_transform."); diff --git a/src/nodelet/edge_detection_nodelet.cpp b/src/nodelet/edge_detection_nodelet.cpp index abdc348e..82d5bf30 100644 --- a/src/nodelet/edge_detection_nodelet.cpp +++ b/src/nodelet/edge_detection_nodelet.cpp @@ -117,20 +117,20 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -205,7 +205,7 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet } case opencv_apps::EdgeDetection_Canny: { - int edgeThresh = 1; + int edge_thresh = 1; int kernel_size = 3; int const max_canny_threshold1 = 500; int const max_canny_threshold2 = 500; @@ -273,7 +273,7 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -282,7 +282,7 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &EdgeDetectionNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -290,7 +290,7 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -327,7 +327,7 @@ namespace edge_detection class EdgeDetectionNodelet : public opencv_apps::EdgeDetectionNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet edge_detection/edge_detection is deprecated, " "and renamed to opencv_apps/edge_detection."); diff --git a/src/nodelet/face_detection_nodelet.cpp b/src/nodelet/face_detection_nodelet.cpp index 5d0f9d84..d3c5376c 100644 --- a/src/nodelet/face_detection_nodelet.cpp +++ b/src/nodelet/face_detection_nodelet.cpp @@ -94,15 +94,15 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -135,51 +135,51 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet #endif cv::Mat face_image; - if (faces.size() > 0) + if (!faces.empty()) { cv::Rect max_area = faces[0]; - for (size_t i = 0; i < faces.size(); i++) + for (const cv::Rect& face : faces) { - if (max_area.width * max_area.height > faces[i].width * faces[i].height) + if (max_area.width * max_area.height > face.width * face.height) { - max_area = faces[i]; + max_area = face; } } face_image = frame(max_area).clone(); } - for (size_t i = 0; i < faces.size(); i++) + for (const cv::Rect& face : faces) { - cv::Point center(faces[i].x + faces[i].width / 2, faces[i].y + faces[i].height / 2); - cv::ellipse(frame, center, cv::Size(faces[i].width / 2, faces[i].height / 2), 0, 0, 360, - cv::Scalar(255, 0, 255), 2, 8, 0); + cv::Point center(face.x + face.width / 2, face.y + face.height / 2); + cv::ellipse(frame, center, cv::Size(face.width / 2, face.height / 2), 0, 0, 360, cv::Scalar(255, 0, 255), 2, 8, + 0); opencv_apps::Face face_msg; face_msg.face.x = center.x; face_msg.face.y = center.y; - face_msg.face.width = faces[i].width; - face_msg.face.height = faces[i].height; + face_msg.face.width = face.width; + face_msg.face.height = face.height; - cv::Mat faceROI = frame_gray(faces[i]); + cv::Mat face_roi = frame_gray(face); std::vector eyes; //-- In each face, detect eyes #ifndef CV_VERSION_EPOCH - eyes_cascade_.detectMultiScale(faceROI, eyes, 1.1, 2, 0, cv::Size(30, 30)); + eyes_cascade_.detectMultiScale(face_roi, eyes, 1.1, 2, 0, cv::Size(30, 30)); #else - eyes_cascade_.detectMultiScale(faceROI, eyes, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30)); + eyes_cascade_.detectMultiScale(face_roi, eyes, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30)); #endif - for (size_t j = 0; j < eyes.size(); j++) + for (const cv::Rect& eye : eyes) { - cv::Point eye_center(faces[i].x + eyes[j].x + eyes[j].width / 2, faces[i].y + eyes[j].y + eyes[j].height / 2); - int radius = cvRound((eyes[j].width + eyes[j].height) * 0.25); + cv::Point eye_center(face.x + eye.x + eye.width / 2, face.y + eye.y + eye.height / 2); + int radius = cvRound((eye.width + eye.height) * 0.25); cv::circle(frame, eye_center, radius, cv::Scalar(255, 0, 0), 3, 8, 0); opencv_apps::Rect eye_msg; eye_msg.x = eye_center.x; eye_msg.y = eye_center.y; - eye_msg.width = eyes[j].width; - eye_msg.height = eyes[j].height; + eye_msg.width = eye.width; + eye_msg.height = eye.height; face_msg.eyes.push_back(eye_msg); } @@ -197,7 +197,7 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg(); img_pub_.publish(out_img); msg_pub_.publish(faces_msg); - if (faces.size() > 0) + if (!faces.empty()) { sensor_msgs::Image::Ptr out_face_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, face_image).toImageMsg(); @@ -212,7 +212,7 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -221,7 +221,7 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &FaceDetectionNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -229,7 +229,7 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -276,7 +276,7 @@ namespace face_detection class FaceDetectionNodelet : public opencv_apps::FaceDetectionNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet face_detection/face_detection is deprecated, " "and renamed to opencv_apps/face_detection."); diff --git a/src/nodelet/face_recognition_nodelet.cpp b/src/nodelet/face_recognition_nodelet.cpp index 9ed51994..244dacd3 100644 --- a/src/nodelet/face_recognition_nodelet.cpp +++ b/src/nodelet/face_recognition_nodelet.cpp @@ -103,7 +103,7 @@ path user_expanded_path(const path& p) if (user_dir.length() == 1) { homedir = getenv("HOME"); - if (homedir == NULL) + if (homedir == nullptr) { homedir = getpwuid(getuid())->pw_dir; } @@ -112,15 +112,15 @@ path user_expanded_path(const path& p) { std::string uname = user_dir.substr(1, user_dir.length()); passwd* pw = getpwnam(uname.c_str()); - if (pw == NULL) + if (pw == nullptr) return p; homedir = pw->pw_dir; } ret = path(std::string(homedir)); return ret.append(++it, p.end(), path::codecvt()); } -} -} // end of utility for resolving paths +} // namespace filesystem +} // namespace boost namespace opencv_apps { @@ -137,11 +137,11 @@ class LabelMapper if (id < it->second) id = it->second + 1; } - for (size_t i = 0; i < l.size(); ++i) + for (const std::string& i : l) { - if (m_.find(l[i]) == m_.end()) + if (m_.find(i) == m_.end()) { - m_[l[i]] = id; + m_[i] = id; id++; } } @@ -372,7 +372,7 @@ class FaceRecognitionNodelet : public opencv_apps::Nodelet storage_->load(images, labels); } - if (images.size() == 0) + if (images.empty()) return; std::vector resized_images(images.size()); @@ -461,7 +461,7 @@ class FaceRecognitionNodelet : public opencv_apps::Nodelet try { std::vector images(req.images.size()); - bool use_roi = req.rects.size() == 0 ? false : true; + bool use_roi = !req.rects.empty(); if (use_roi && req.images.size() != req.rects.size()) { @@ -617,7 +617,7 @@ class FaceRecognitionNodelet : public opencv_apps::Nodelet config_ = config; } - void subscribe() + void subscribe() override { NODELET_DEBUG("subscribe"); img_sub_.subscribe(*it_, "image", 1); @@ -636,7 +636,7 @@ class FaceRecognitionNodelet : public opencv_apps::Nodelet } } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("unsubscribe"); img_sub_.unsubscribe(); @@ -644,7 +644,7 @@ class FaceRecognitionNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); @@ -676,7 +676,7 @@ namespace face_recognition class FaceRecognitionNodelet : public opencv_apps::FaceRecognitionNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet face_recognition/face_recognition is deprecated, " "and renamed to opencv_apps/face_recognition."); diff --git a/src/nodelet/fback_flow_nodelet.cpp b/src/nodelet/fback_flow_nodelet.cpp index 604e1b32..724e9868 100644 --- a/src/nodelet/fback_flow_nodelet.cpp +++ b/src/nodelet/fback_flow_nodelet.cpp @@ -91,20 +91,20 @@ class FBackFlowNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -191,7 +191,7 @@ class FBackFlowNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -200,7 +200,7 @@ class FBackFlowNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &FBackFlowNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -208,7 +208,7 @@ class FBackFlowNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -242,7 +242,7 @@ namespace fback_flow class FBackFlowNodelet : public opencv_apps::FBackFlowNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet fback_flow/fback_flow is deprecated, " "and renamed to opencv_apps/fback_flow."); diff --git a/src/nodelet/find_contours_nodelet.cpp b/src/nodelet/find_contours_nodelet.cpp index 732c6f33..0e05b1bc 100644 --- a/src/nodelet/find_contours_nodelet.cpp +++ b/src/nodelet/find_contours_nodelet.cpp @@ -95,20 +95,20 @@ class FindContoursNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -163,11 +163,11 @@ class FindContoursNodelet : public opencv_apps::Nodelet cv::drawContours(drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point()); opencv_apps::Contour contour_msg; - for (size_t j = 0; j < contours[i].size(); j++) + for (const cv::Point& j : contours[i]) { opencv_apps::Point2D point_msg; - point_msg.x = contours[i][j].x; - point_msg.y = contours[i][j].y; + point_msg.x = j.x; + point_msg.y = j.y; contour_msg.points.push_back(point_msg); } contours_msg.contours.push_back(contour_msg); @@ -202,7 +202,7 @@ class FindContoursNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -211,7 +211,7 @@ class FindContoursNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &FindContoursNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -219,7 +219,7 @@ class FindContoursNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -254,7 +254,7 @@ namespace find_contours class FindContoursNodelet : public opencv_apps::FindContoursNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet find_contours/find_contours is deprecated, " "and renamed to opencv_apps/find_contours."); diff --git a/src/nodelet/general_contours_nodelet.cpp b/src/nodelet/general_contours_nodelet.cpp index aaf18a6b..dc886720 100644 --- a/src/nodelet/general_contours_nodelet.cpp +++ b/src/nodelet/general_contours_nodelet.cpp @@ -95,20 +95,20 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -154,15 +154,15 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet cv::findContours(threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0)); /// Find the rotated rectangles and ellipses for each contour - std::vector minRect(contours.size()); - std::vector minEllipse(contours.size()); + std::vector min_rect(contours.size()); + std::vector min_ellipse(contours.size()); for (size_t i = 0; i < contours.size(); i++) { - minRect[i] = cv::minAreaRect(cv::Mat(contours[i])); + min_rect[i] = cv::minAreaRect(cv::Mat(contours[i])); if (contours[i].size() > 5) { - minEllipse[i] = cv::fitEllipse(cv::Mat(contours[i])); + min_ellipse[i] = cv::fitEllipse(cv::Mat(contours[i])); } } @@ -174,33 +174,33 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet // contour cv::drawContours(drawing, contours, (int)i, color, 1, 8, std::vector(), 0, cv::Point()); // ellipse - cv::ellipse(drawing, minEllipse[i], color, 2, 8); + cv::ellipse(drawing, min_ellipse[i], color, 2, 8); // rotated rectangle cv::Point2f rect_points[4]; - minRect[i].points(rect_points); + min_rect[i].points(rect_points); for (int j = 0; j < 4; j++) cv::line(drawing, rect_points[j], rect_points[(j + 1) % 4], color, 1, 8); opencv_apps::RotatedRect rect_msg; opencv_apps::Point2D rect_center; opencv_apps::Size rect_size; - rect_center.x = minRect[i].center.x; - rect_center.y = minRect[i].center.y; - rect_size.width = minRect[i].size.width; - rect_size.height = minRect[i].size.height; + rect_center.x = min_rect[i].center.x; + rect_center.y = min_rect[i].center.y; + rect_size.width = min_rect[i].size.width; + rect_size.height = min_rect[i].size.height; rect_msg.center = rect_center; rect_msg.size = rect_size; - rect_msg.angle = minRect[i].angle; + rect_msg.angle = min_rect[i].angle; opencv_apps::RotatedRect ellipse_msg; opencv_apps::Point2D ellipse_center; opencv_apps::Size ellipse_size; - ellipse_center.x = minEllipse[i].center.x; - ellipse_center.y = minEllipse[i].center.y; - ellipse_size.width = minEllipse[i].size.width; - ellipse_size.height = minEllipse[i].size.height; + ellipse_center.x = min_ellipse[i].center.x; + ellipse_center.y = min_ellipse[i].center.y; + ellipse_size.width = min_ellipse[i].size.width; + ellipse_size.height = min_ellipse[i].size.height; ellipse_msg.center = ellipse_center; ellipse_msg.size = ellipse_size; - ellipse_msg.angle = minEllipse[i].angle; + ellipse_msg.angle = min_ellipse[i].angle; rects_msg.rects.push_back(rect_msg); ellipses_msg.rects.push_back(ellipse_msg); @@ -236,7 +236,7 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -245,7 +245,7 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &GeneralContoursNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -253,7 +253,7 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -289,7 +289,7 @@ namespace general_contours class GeneralContoursNodelet : public opencv_apps::GeneralContoursNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet general_contours/general_contours is deprecated, " "and renamed to opencv_apps/general_contours."); diff --git a/src/nodelet/goodfeature_track_nodelet.cpp b/src/nodelet/goodfeature_track_nodelet.cpp index 406f9532..5c63924f 100644 --- a/src/nodelet/goodfeature_track_nodelet.cpp +++ b/src/nodelet/goodfeature_track_nodelet.cpp @@ -93,20 +93,20 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -120,7 +120,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet // Do the work cv::Mat src_gray; - int maxTrackbar = 100; + int max_trackbar = 100; if (frame.channels() > 1) { @@ -136,7 +136,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet { /// Create Trackbars for Thresholds cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); - cv::createTrackbar("Max corners", window_name_, &max_corners_, maxTrackbar, trackbarCallback); + cv::createTrackbar("Max corners", window_name_, &max_corners_, max_trackbar, trackbarCallback); if (need_config_update_) { config_.max_corners = max_corners_; @@ -153,25 +153,25 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet /// Parameters for Shi-Tomasi algorithm std::vector corners; - double qualityLevel = 0.01; - double minDistance = 10; - int blockSize = 3; - bool useHarrisDetector = false; + double quality_level = 0.01; + double min_distance = 10; + int block_size = 3; + bool use_harris_detector = false; double k = 0.04; cv::RNG rng(12345); /// Apply corner detection - cv::goodFeaturesToTrack(src_gray, corners, max_corners_, qualityLevel, minDistance, cv::Mat(), blockSize, - useHarrisDetector, k); + cv::goodFeaturesToTrack(src_gray, corners, max_corners_, quality_level, min_distance, cv::Mat(), block_size, + use_harris_detector, k); /// Draw corners detected NODELET_INFO_STREAM("** Number of corners detected: " << corners.size()); int r = 4; - for (size_t i = 0; i < corners.size(); i++) + for (const cv::Point2f& corner : corners) { - cv::circle(frame, corners[i], r, cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), -1, - 8, 0); + cv::circle(frame, corner, r, cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), -1, 8, + 0); } //-- Show what you got @@ -182,11 +182,11 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet } // Create msgs - for (size_t i = 0; i < corners.size(); i++) + for (const cv::Point2f& i : corners) { opencv_apps::Point2D corner; - corner.x = corners[i].x; - corner.y = corners[i].y; + corner.x = i.x; + corner.y = i.y; corners_msg.points.push_back(corner); } // Publish the image. @@ -202,7 +202,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -211,7 +211,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &GoodfeatureTrackNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -219,7 +219,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -254,7 +254,7 @@ namespace goodfeature_track class GoodfeatureTrackNodelet : public opencv_apps::GoodfeatureTrackNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet goodfeature_track/goodfeature_track is deprecated, " "and renamed to opencv_apps/goodfeature_track."); diff --git a/src/nodelet/hough_circles_nodelet.cpp b/src/nodelet/hough_circles_nodelet.cpp index 687f606b..681c191a 100644 --- a/src/nodelet/hough_circles_nodelet.cpp +++ b/src/nodelet/hough_circles_nodelet.cpp @@ -134,12 +134,12 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } static void trackbarCallback(int value, void* userdata) @@ -147,7 +147,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -250,10 +250,10 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet } // clone the colour, input image for displaying purposes - for (size_t i = 0; i < circles.size(); i++) + for (const cv::Vec3f& i : circles) { - cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1])); - int radius = cvRound(circles[i][2]); + cv::Point center(cvRound(i[0]), cvRound(i[1])); + int radius = cvRound(i[2]); // circle center circle(out_image, center, 3, cv::Scalar(0, 255, 0), -1, 8, 0); // circle outline @@ -314,7 +314,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -323,7 +323,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &HoughCirclesNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -331,7 +331,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -378,7 +378,7 @@ namespace hough_circles class HoughCirclesNodelet : public opencv_apps::HoughCirclesNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet hough_circles/hough_circles is deprecated, " "and renamed to opencv_apps/hough_circles."); diff --git a/src/nodelet/hough_lines_nodelet.cpp b/src/nodelet/hough_lines_nodelet.cpp index e50ae958..98fec48a 100644 --- a/src/nodelet/hough_lines_nodelet.cpp +++ b/src/nodelet/hough_lines_nodelet.cpp @@ -105,20 +105,20 @@ class HoughLinesNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -195,9 +195,9 @@ class HoughLinesNodelet : public opencv_apps::Nodelet cv::HoughLines(in_image, s_lines, rho_, theta_ * CV_PI / 180, threshold_, minLineLength_, maxLineGap_); /// Show the result - for (size_t i = 0; i < s_lines.size(); i++) + for (const cv::Vec2f& s_line : s_lines) { - float r = s_lines[i][0], t = s_lines[i][1]; + float r = s_line[0], t = s_line[1]; double cos_t = cos(t), sin_t = sin(t); double x0 = r * cos_t, y0 = r * sin_t; double alpha = 1000; @@ -228,9 +228,8 @@ class HoughLinesNodelet : public opencv_apps::Nodelet cv::HoughLinesP(in_image, p_lines, rho_, theta_ * CV_PI / 180, threshold_, minLineLength_, maxLineGap_); /// Show the result - for (size_t i = 0; i < p_lines.size(); i++) + for (const cv::Vec4i& l : p_lines) { - cv::Vec4i l = p_lines[i]; #ifndef CV_VERSION_EPOCH cv::line(out_image, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255, 0, 0), 3, cv::LINE_AA); #else @@ -268,7 +267,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -277,7 +276,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &HoughLinesNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -285,7 +284,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -322,7 +321,7 @@ namespace hough_lines class HoughLinesNodelet : public opencv_apps::HoughLinesNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet hough_lines/hough_lines is deprecated, " "and renamed to opencv_apps/hough_lines."); diff --git a/src/nodelet/lk_flow_nodelet.cpp b/src/nodelet/lk_flow_nodelet.cpp index f08b3f53..0d17bb8c 100644 --- a/src/nodelet/lk_flow_nodelet.cpp +++ b/src/nodelet/lk_flow_nodelet.cpp @@ -110,12 +110,12 @@ class LKFlowNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } #if 0 static void onMouse( int event, int x, int y, int /*flags*/, void* /*param*/ ) @@ -127,12 +127,12 @@ class LKFlowNodelet : public opencv_apps::Nodelet } } #endif - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -166,7 +166,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet // Do the work cv::TermCriteria termcrit(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 20, 0.03); - cv::Size subPixWinSize(10, 10), winSize(31, 31); + cv::Size sub_pix_win_size(10, 10), win_size(31, 31); if (image.channels() > 1) { @@ -185,7 +185,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet // automatic initialization cv::goodFeaturesToTrack(gray, points[1], MAX_COUNT, quality_level_, min_distance_, cv::Mat(), block_size_, false, harris_k_); - cv::cornerSubPix(gray, points[1], subPixWinSize, cv::Size(-1, -1), termcrit); + cv::cornerSubPix(gray, points[1], sub_pix_win_size, cv::Size(-1, -1), termcrit); addRemovePt = false; } else if (!points[0].empty()) @@ -194,7 +194,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet std::vector err; if (prevGray.empty()) gray.copyTo(prevGray); - cv::calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize, 3, termcrit, 0, 0.001); + cv::calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, win_size, 3, termcrit, 0, 0.001); size_t i, k; for (i = k = 0; i < points[1].size(); i++) { @@ -232,7 +232,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet { std::vector tmp; tmp.push_back(point); - cv::cornerSubPix(gray, tmp, winSize, cv::Size(-1, -1), termcrit); + cv::cornerSubPix(gray, tmp, win_size, cv::Size(-1, -1), termcrit); points[1].push_back(tmp[0]); addRemovePt = false; } @@ -275,26 +275,26 @@ class LKFlowNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - bool initialize_points_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) + bool initializePointsCb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { needToInit = true; return true; } - bool delete_points_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) + bool deletePointsCb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { points[0].clear(); points[1].clear(); return true; } - bool toggle_night_mode_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) + bool toggleNightModeCb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { nightMode = !nightMode; return true; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -303,7 +303,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &LKFlowNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -311,7 +311,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -337,11 +337,9 @@ class LKFlowNodelet : public opencv_apps::Nodelet img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "flows", 1); - initialize_points_service_ = - pnh_->advertiseService("initialize_points", &LKFlowNodelet::initialize_points_cb, this); - delete_points_service_ = pnh_->advertiseService("delete_points", &LKFlowNodelet::delete_points_cb, this); - toggle_night_mode_service_ = - pnh_->advertiseService("toggle_night_mode", &LKFlowNodelet::toggle_night_mode_cb, this); + initialize_points_service_ = pnh_->advertiseService("initialize_points", &LKFlowNodelet::initializePointsCb, this); + delete_points_service_ = pnh_->advertiseService("delete_points", &LKFlowNodelet::deletePointsCb, this); + toggle_night_mode_service_ = pnh_->advertiseService("toggle_night_mode", &LKFlowNodelet::toggleNightModeCb, this); NODELET_INFO("Hot keys: "); NODELET_INFO("\tESC - quit the program"); @@ -361,7 +359,7 @@ namespace lk_flow class LKFlowNodelet : public opencv_apps::LKFlowNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet lk_flow/lk_flow is deprecated, " "and renamed to opencv_apps/lk_flow."); diff --git a/src/nodelet/nodelet.cpp b/src/nodelet/nodelet.cpp index af5b30a6..b4175e99 100644 --- a/src/nodelet/nodelet.cpp +++ b/src/nodelet/nodelet.cpp @@ -79,9 +79,8 @@ void Nodelet::connectionCallback(const ros::SingleSubscriberPublisher& pub) if (!always_subscribe_) { boost::mutex::scoped_lock lock(connection_mutex_); - for (size_t i = 0; i < publishers_.size(); i++) + for (const ros::Publisher& pub : publishers_) { - ros::Publisher pub = publishers_[i]; if (pub.getNumSubscribers() > 0) { if (!ever_subscribed_) @@ -121,9 +120,8 @@ void Nodelet::imageConnectionCallback(const image_transport::SingleSubscriberPub if (!always_subscribe_) { boost::mutex::scoped_lock lock(connection_mutex_); - for (size_t i = 0; i < image_publishers_.size(); i++) + for (const image_transport::Publisher& pub : image_publishers_) { - image_transport::Publisher pub = image_publishers_[i]; if (pub.getNumSubscribers() > 0) { if (!ever_subscribed_) @@ -173,9 +171,8 @@ void Nodelet::cameraConnectionBaseCallback() if (!always_subscribe_) { boost::mutex::scoped_lock lock(connection_mutex_); - for (size_t i = 0; i < camera_publishers_.size(); i++) + for (const image_transport::CameraPublisher& pub : camera_publishers_) { - image_transport::CameraPublisher pub = camera_publishers_[i]; if (pub.getNumSubscribers() > 0) { if (!ever_subscribed_) @@ -205,4 +202,4 @@ void Nodelet::cameraConnectionBaseCallback() } } } -} +} // namespace opencv_apps diff --git a/src/nodelet/people_detect_nodelet.cpp b/src/nodelet/people_detect_nodelet.cpp index 8ac6e42e..45171780 100644 --- a/src/nodelet/people_detect_nodelet.cpp +++ b/src/nodelet/people_detect_nodelet.cpp @@ -103,20 +103,20 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -192,7 +192,7 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -201,7 +201,7 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &PeopleDetectNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -209,7 +209,7 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -245,7 +245,7 @@ namespace people_detect class PeopleDetectNodelet : public opencv_apps::PeopleDetectNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet people_detect/people_detect is deprecated, " "and renamed to opencv_apps/people_detect."); diff --git a/src/nodelet/phase_corr_nodelet.cpp b/src/nodelet/phase_corr_nodelet.cpp index 8455f108..f6e17c39 100644 --- a/src/nodelet/phase_corr_nodelet.cpp +++ b/src/nodelet/phase_corr_nodelet.cpp @@ -86,20 +86,20 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -184,7 +184,7 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -193,7 +193,7 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &PhaseCorrNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -201,7 +201,7 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -235,7 +235,7 @@ namespace phase_corr class PhaseCorrNodelet : public opencv_apps::PhaseCorrNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet phase_corr/phase_corr is deprecated, " "and renamed to opencv_apps/phase_corr."); diff --git a/src/nodelet/pyramids_nodelet.cpp b/src/nodelet/pyramids_nodelet.cpp index b72c29b4..8eeba0d4 100644 --- a/src/nodelet/pyramids_nodelet.cpp +++ b/src/nodelet/pyramids_nodelet.cpp @@ -91,15 +91,15 @@ class PyramidsNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - void do_work(const sensor_msgs::ImageConstPtr& image_msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& image_msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -148,7 +148,7 @@ class PyramidsNodelet : public opencv_apps::Nodelet } } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -157,7 +157,7 @@ class PyramidsNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &PyramidsNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -165,7 +165,7 @@ class PyramidsNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -197,7 +197,7 @@ namespace pyramids class PyramidsNodelet : public opencv_apps::PyramidsNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet pyramids/pyramids is deprecated, " "and renamed to opencv_apps/pyramids."); diff --git a/src/nodelet/segment_objects_nodelet.cpp b/src/nodelet/segment_objects_nodelet.cpp index 359361d5..58e89d78 100644 --- a/src/nodelet/segment_objects_nodelet.cpp +++ b/src/nodelet/segment_objects_nodelet.cpp @@ -100,20 +100,20 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -160,37 +160,37 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet out_frame = cv::Mat::zeros(frame.size(), CV_8UC3); - if (contours.size() == 0) + if (contours.empty()) return; // iterate through all the top-level contours, // draw each connected component with its own random color - int idx = 0, largestComp = 0; - double maxArea = 0; + int idx = 0, largest_comp = 0; + double max_area = 0; for (; idx >= 0; idx = hierarchy[idx][0]) { const std::vector& c = contours[idx]; double area = fabs(cv::contourArea(cv::Mat(c))); - if (area > maxArea) + if (area > max_area) { - maxArea = area; - largestComp = idx; + max_area = area; + largest_comp = idx; } } cv::Scalar color(0, 0, 255); - cv::drawContours(out_frame, contours, largestComp, color, CV_FILLED, 8, hierarchy); + cv::drawContours(out_frame, contours, largest_comp, color, CV_FILLED, 8, hierarchy); std_msgs::Float64 area_msg; - area_msg.data = maxArea; - for (size_t i = 0; i < contours.size(); i++) + area_msg.data = max_area; + for (const std::vector& contour : contours) { opencv_apps::Contour contour_msg; - for (size_t j = 0; j < contours[i].size(); j++) + for (const cv::Point& j : contour) { opencv_apps::Point2D point_msg; - point_msg.x = contours[i][j].x; - point_msg.y = contours[i][j].y; + point_msg.x = j.x; + point_msg.y = j.y; contour_msg.points.push_back(point_msg); } contours_msg.contours.push_back(contour_msg); @@ -225,14 +225,14 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - bool update_bg_model_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) + bool updateBgModelCb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { update_bg_model = !update_bg_model; NODELET_INFO("Learn background is in state = %d", update_bg_model); return true; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -241,7 +241,7 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &SegmentObjectsNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -249,7 +249,7 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -279,8 +279,7 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "contours", 1); area_pub_ = advertise(*pnh_, "area", 1); - update_bg_model_service_ = - pnh_->advertiseService("update_bg_model", &SegmentObjectsNodelet::update_bg_model_cb, this); + update_bg_model_service_ = pnh_->advertiseService("update_bg_model", &SegmentObjectsNodelet::updateBgModelCb, this); onInitPostProcess(); } @@ -293,7 +292,7 @@ namespace segment_objects class SegmentObjectsNodelet : public opencv_apps::SegmentObjectsNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet segment_objects/segment_objects is deprecated, " "and renamed to opencv_apps/segment_objects."); diff --git a/src/nodelet/simple_compressed_example_nodelet.cpp b/src/nodelet/simple_compressed_example_nodelet.cpp index 0a940884..610af320 100644 --- a/src/nodelet/simple_compressed_example_nodelet.cpp +++ b/src/nodelet/simple_compressed_example_nodelet.cpp @@ -71,9 +71,9 @@ class ImageConverter image_sub_ = nh_.subscribe("image/compressed", queue_size_, &ImageConverter::imageCb, this); image_pub_ = nh_.advertise("/image_converter/output_video/compressed", 1); - ros::NodeHandle pnh_("~"); - pnh_.param("queue_size", queue_size_, 3); - pnh_.param("debug_view", debug_view_, false); + ros::NodeHandle pnh("~"); + pnh.param("queue_size", queue_size_, 3); + pnh.param("debug_view", debug_view_, false); if (debug_view_) { cv::namedWindow(OPENCV_WINDOW); @@ -226,7 +226,7 @@ note : hydro 1.10.18, indigo : 1.11.13 ... class SimpleCompressedExampleNodelet : public nodelet::Nodelet { public: - virtual void onInit() + void onInit() override { simple_compressed_example::ImageConverter ic; ros::spin(); @@ -240,7 +240,7 @@ namespace simple_compressed_example class SimpleCompressedExampleNodelet : public opencv_apps::SimpleCompressedExampleNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet simple_compressed_example/simple_compressed_example is deprecated, " "and renamed to opencv_apps/simple_compressed_example."); diff --git a/src/nodelet/simple_example_nodelet.cpp b/src/nodelet/simple_example_nodelet.cpp index 7389bfe6..c34a2c22 100644 --- a/src/nodelet/simple_example_nodelet.cpp +++ b/src/nodelet/simple_example_nodelet.cpp @@ -69,9 +69,9 @@ class ImageConverter image_sub_ = it_.subscribe("image", queue_size_, &ImageConverter::imageCb, this); image_pub_ = it_.advertise("/image_converter/output_video/raw", 1); - ros::NodeHandle pnh_("~"); - pnh_.param("queue_size", queue_size_, 1); - pnh_.param("debug_view", debug_view_, false); + ros::NodeHandle pnh("~"); + pnh.param("queue_size", queue_size_, 1); + pnh.param("debug_view", debug_view_, false); if (debug_view_) { cv::namedWindow(OPENCV_WINDOW); @@ -115,12 +115,12 @@ class ImageConverter } }; -} // namesapce simple_example +} // namespace simple_example class SimpleExampleNodelet : public nodelet::Nodelet { public: - virtual void onInit() + void onInit() override { simple_example::ImageConverter ic; ros::spin(); @@ -134,7 +134,7 @@ namespace simple_example class SimpleExampleNodelet : public opencv_apps::SimpleExampleNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet simple_example/simple_example is deprecated, " "and renamed to opencv_apps/simple_example."); diff --git a/src/nodelet/simple_flow_nodelet.cpp b/src/nodelet/simple_flow_nodelet.cpp index 2512dd30..a8b27743 100644 --- a/src/nodelet/simple_flow_nodelet.cpp +++ b/src/nodelet/simple_flow_nodelet.cpp @@ -96,20 +96,20 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -224,7 +224,7 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -233,7 +233,7 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &SimpleFlowNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -241,7 +241,7 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -276,7 +276,7 @@ namespace simple_flow class SimpleFlowNodelet : public opencv_apps::SimpleFlowNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet simple_flow/simple_flow is deprecated, " "and renamed to opencv_apps/simple_flow."); diff --git a/src/nodelet/smoothing_nodelet.cpp b/src/nodelet/smoothing_nodelet.cpp index aab5e115..c1d582a9 100644 --- a/src/nodelet/smoothing_nodelet.cpp +++ b/src/nodelet/smoothing_nodelet.cpp @@ -99,20 +99,20 @@ class SmoothingNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -189,7 +189,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -198,7 +198,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &SmoothingNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -206,7 +206,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -240,7 +240,7 @@ namespace smoothing class SmoothingNodelet : public opencv_apps::SmoothingNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet smoothing/smoothing is deprecated, " "and renamed to opencv_apps/smoothing."); diff --git a/src/nodelet/threshold_nodelet.cpp b/src/nodelet/threshold_nodelet.cpp index 32416145..35e59985 100644 --- a/src/nodelet/threshold_nodelet.cpp +++ b/src/nodelet/threshold_nodelet.cpp @@ -82,15 +82,15 @@ class ThresholdNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -99,7 +99,7 @@ class ThresholdNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &ThresholdNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -116,7 +116,7 @@ class ThresholdNodelet : public opencv_apps::Nodelet apply_otsu_ = config.apply_otsu; } - void do_work(const sensor_msgs::Image::ConstPtr& image_msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::Image::ConstPtr& image_msg, const std::string& input_frame_from_msg) { try { @@ -147,7 +147,7 @@ class ThresholdNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -177,7 +177,7 @@ namespace threshold class ThresholdNodelet : public opencv_apps::ThresholdNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet threshold/threshold is deprecated, " "and renamed to opencv_apps/threshold."); diff --git a/src/nodelet/watershed_segmentation_nodelet.cpp b/src/nodelet/watershed_segmentation_nodelet.cpp index c16a8a71..97fada5f 100644 --- a/src/nodelet/watershed_segmentation_nodelet.cpp +++ b/src/nodelet/watershed_segmentation_nodelet.cpp @@ -85,7 +85,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet cv::Mat markerMask; cv::Point prevPt; - static void onMouse(int event, int x, int y, int flags, void*) + static void onMouse(int event, int x, int y, int flags, void* /*unused*/) { on_mouse_update_ = true; on_mouse_event_ = event; @@ -108,20 +108,20 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) { - do_work(msg, cam_info->header.frame_id); + doWork(msg, cam_info->header.frame_id); } void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - do_work(msg, msg->header.frame_id); + doWork(msg, msg->header.frame_id); } - static void trackbarCallback(int, void*) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; } - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { // Work on the image. try @@ -135,20 +135,20 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet // Do the work // std::vector faces; - cv::Mat imgGray; + cv::Mat img_gray; /// Initialize if (markerMask.empty()) { cv::cvtColor(frame, markerMask, cv::COLOR_BGR2GRAY); - cv::cvtColor(markerMask, imgGray, cv::COLOR_GRAY2BGR); + cv::cvtColor(markerMask, img_gray, cv::COLOR_GRAY2BGR); markerMask = cv::Scalar::all(0); } if (debug_view_) { cv::imshow(window_name_, frame); - cv::setMouseCallback(window_name_, onMouse, 0); + cv::setMouseCallback(window_name_, onMouse, nullptr); if (need_config_update_) { reconfigure_server_->updateConfig(config_); @@ -182,7 +182,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet cv::waitKey(1); } - int i, j, compCount = 0; + int i, j, comp_count = 0; std::vector > contours; std::vector hierarchy; @@ -196,35 +196,35 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet cv::Mat markers(markerMask.size(), CV_32S); markers = cv::Scalar::all(0); int idx = 0; - for (; idx >= 0; idx = hierarchy[idx][0], compCount++) - cv::drawContours(markers, contours, idx, cv::Scalar::all(compCount + 1), -1, 8, hierarchy, INT_MAX); + for (; idx >= 0; idx = hierarchy[idx][0], comp_count++) + cv::drawContours(markers, contours, idx, cv::Scalar::all(comp_count + 1), -1, 8, hierarchy, INT_MAX); - if (compCount == 0) + if (comp_count == 0) { NODELET_WARN("compCount is 0"); return; // continue; } - for (size_t i = 0; i < contours.size(); i++) + for (const std::vector& contour : contours) { opencv_apps::Contour contour_msg; - for (size_t j = 0; j < contours[i].size(); j++) + for (const cv::Point& j : contour) { opencv_apps::Point2D point_msg; - point_msg.x = contours[i][j].x; - point_msg.y = contours[i][j].y; + point_msg.x = j.x; + point_msg.y = j.y; contour_msg.points.push_back(point_msg); } contours_msg.contours.push_back(contour_msg); } - std::vector colorTab; - for (i = 0; i < compCount; i++) + std::vector color_tab; + for (i = 0; i < comp_count; i++) { int b = cv::theRNG().uniform(0, 255); int g = cv::theRNG().uniform(0, 255); int r = cv::theRNG().uniform(0, 255); - colorTab.push_back(cv::Vec3b((uchar)b, (uchar)g, (uchar)r)); + color_tab.push_back(cv::Vec3b((uchar)b, (uchar)g, (uchar)r)); } double t = (double)cv::getTickCount(); @@ -241,13 +241,13 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet int index = markers.at(i, j); if (index == -1) wshed.at(i, j) = cv::Vec3b(255, 255, 255); - else if (index <= 0 || index > compCount) + else if (index <= 0 || index > comp_count) wshed.at(i, j) = cv::Vec3b(0, 0, 0); else - wshed.at(i, j) = colorTab[index - 1]; + wshed.at(i, j) = color_tab[index - 1]; } - wshed = wshed * 0.5 + imgGray * 0.5; + wshed = wshed * 0.5 + img_gray * 0.5; //-- Show what you got if (debug_view_) @@ -276,24 +276,24 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void add_seed_point_cb(const opencv_apps::Point2DArray& msg) + void addSeedPointCb(const opencv_apps::Point2DArray& msg) { - if (msg.points.size() == 0) + if (msg.points.empty()) { markerMask = cv::Scalar::all(0); } else { - for (size_t i = 0; i < msg.points.size(); i++) + for (const opencv_apps::Point2D& point : msg.points) { - cv::Point pt0(msg.points[i].x, msg.points[i].y); + cv::Point pt0(point.x, point.y); cv::Point pt1(pt0.x + 1, pt0.y + 1); cv::line(markerMask, pt0, pt1, cv::Scalar::all(255), 5, 8, 0); } } } - void subscribe() + void subscribe() override { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -302,7 +302,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &WatershedSegmentationNodelet::imageCallback, this); } - void unsubscribe() + void unsubscribe() override { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -310,7 +310,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet } public: - virtual void onInit() + void onInit() override { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -333,8 +333,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet boost::bind(&WatershedSegmentationNodelet::reconfigureCallback, this, _1, _2); reconfigure_server_->setCallback(f); - add_seed_points_sub_ = - pnh_->subscribe("add_seed_points", 1, &WatershedSegmentationNodelet::add_seed_point_cb, this); + add_seed_points_sub_ = pnh_->subscribe("add_seed_points", 1, &WatershedSegmentationNodelet::addSeedPointCb, this); img_pub_ = advertiseImage(*pnh_, "image", 1); msg_pub_ = advertise(*pnh_, "contours", 1); @@ -362,7 +361,7 @@ namespace watershed_segmentation class WatershedSegmentationNodelet : public opencv_apps::WatershedSegmentationNodelet { public: - virtual void onInit() + void onInit() override { ROS_WARN("DeprecationWarning: Nodelet watershed_segmentation/watershed_segmentation is deprecated, " "and renamed to opencv_apps/watershed_segmentation."); From f1a8c68ddfbb58ea3715b5480ea5e997b066c718 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 22 Apr 2019 05:21:08 +0000 Subject: [PATCH 5/6] clang-tidy code need c++11 --- CMakeLists.txt | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index c5eebb87..852001b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,22 @@ cmake_minimum_required(VERSION 2.8.3) project(opencv_apps) +## https://stackoverflow.com/questions/10984442/how-to-detect-c11-support-of-a-compiler-with-cmake +if(CMAKE_COMPILER_IS_GNUCXX) + execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) + if (GCC_VERSION VERSION_GREATER 4.7 OR GCC_VERSION VERSION_EQUAL 4.7) + message(STATUS "C++11 activated.") + add_definitions("-std=gnu++11") + elseif(GCC_VERSION VERSION_GREATER 4.3 OR GCC_VERSION VERSION_EQUAL 4.3) + message(WARNING "C++0x activated. If you get any errors update to a compiler which fully supports C++11") + add_definitions("-std=gnu++0x") + else () + message(FATAL_ERROR "C++11 needed. Therefore a gcc compiler with a version higher than 4.3 is needed.") + endif() +else(CMAKE_COMPILER_IS_GNUCXX) + add_definitions("-std=c++0x") +endif(CMAKE_COMPILER_IS_GNUCXX) + find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport nodelet roscpp sensor_msgs std_msgs std_srvs) find_package(OpenCV REQUIRED) From 66980a27308b3cc62bc3eaf27492a80520c4622a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 22 Apr 2019 16:01:45 +0900 Subject: [PATCH 6/6] override is not supported gcc4.6 (12.04), remove this fix and add NOLINT --- src/nodelet/adding_images_nodelet.cpp | 8 ++--- src/nodelet/camshift_nodelet.cpp | 8 ++--- src/nodelet/color_filter_nodelet.cpp | 30 +++++++++---------- src/nodelet/contour_moments_nodelet.cpp | 8 ++--- src/nodelet/convex_hull_nodelet.cpp | 8 ++--- src/nodelet/corner_harris_nodelet.cpp | 8 ++--- .../discrete_fourier_transform_nodelet.cpp | 8 ++--- src/nodelet/edge_detection_nodelet.cpp | 8 ++--- src/nodelet/face_detection_nodelet.cpp | 8 ++--- src/nodelet/face_recognition_nodelet.cpp | 8 ++--- src/nodelet/fback_flow_nodelet.cpp | 8 ++--- src/nodelet/find_contours_nodelet.cpp | 8 ++--- src/nodelet/general_contours_nodelet.cpp | 8 ++--- src/nodelet/goodfeature_track_nodelet.cpp | 8 ++--- src/nodelet/hough_circles_nodelet.cpp | 8 ++--- src/nodelet/hough_lines_nodelet.cpp | 8 ++--- src/nodelet/lk_flow_nodelet.cpp | 8 ++--- src/nodelet/people_detect_nodelet.cpp | 8 ++--- src/nodelet/phase_corr_nodelet.cpp | 8 ++--- src/nodelet/pyramids_nodelet.cpp | 8 ++--- src/nodelet/segment_objects_nodelet.cpp | 8 ++--- .../simple_compressed_example_nodelet.cpp | 4 +-- src/nodelet/simple_example_nodelet.cpp | 4 +-- src/nodelet/simple_flow_nodelet.cpp | 8 ++--- src/nodelet/smoothing_nodelet.cpp | 8 ++--- src/nodelet/threshold_nodelet.cpp | 8 ++--- .../watershed_segmentation_nodelet.cpp | 8 ++--- 27 files changed, 115 insertions(+), 115 deletions(-) diff --git a/src/nodelet/adding_images_nodelet.cpp b/src/nodelet/adding_images_nodelet.cpp index 18d2e600..2e83b3e5 100644 --- a/src/nodelet/adding_images_nodelet.cpp +++ b/src/nodelet/adding_images_nodelet.cpp @@ -109,7 +109,7 @@ class AddingImagesNodelet : public opencv_apps::Nodelet doWork(msg1, msg2, msg1->header.frame_id); } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); sub_image1_.subscribe(*it_, "image1", 3); @@ -148,7 +148,7 @@ class AddingImagesNodelet : public opencv_apps::Nodelet } } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); sub_image1_.unsubscribe(); @@ -241,7 +241,7 @@ class AddingImagesNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -275,7 +275,7 @@ namespace adding_images class AddingImagesNodelet : public opencv_apps::AddingImagesNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet adding_images/adding_images is deprecated, " "and renamed to opencv_apps/adding_images."); diff --git a/src/nodelet/camshift_nodelet.cpp b/src/nodelet/camshift_nodelet.cpp index e3210c31..a16cfb62 100644 --- a/src/nodelet/camshift_nodelet.cpp +++ b/src/nodelet/camshift_nodelet.cpp @@ -338,7 +338,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -347,7 +347,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &CamShiftNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -355,7 +355,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -444,7 +444,7 @@ namespace camshift class CamShiftNodelet : public opencv_apps::CamShiftNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet camshift/camshift is deprecated, " "and renamed to opencv_apps/camshift."); diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp index 2939b4d4..90af18e9 100644 --- a/src/nodelet/color_filter_nodelet.cpp +++ b/src/nodelet/color_filter_nodelet.cpp @@ -148,7 +148,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -157,7 +157,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &ColorFilterNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -165,7 +165,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -201,7 +201,7 @@ class RGBColorFilterNodelet : public ColorFilterNodeletheader.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -258,7 +258,7 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &ContourMomentsNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -266,7 +266,7 @@ class ContourMomentsNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -300,7 +300,7 @@ namespace contour_moments class ContourMomentsNodelet : public opencv_apps::ContourMomentsNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet contour_moments/contour_moments is deprecated, " "and renamed to opencv_apps/contour_moments."); diff --git a/src/nodelet/convex_hull_nodelet.cpp b/src/nodelet/convex_hull_nodelet.cpp index e2fb95d4..93871823 100644 --- a/src/nodelet/convex_hull_nodelet.cpp +++ b/src/nodelet/convex_hull_nodelet.cpp @@ -207,7 +207,7 @@ class ConvexHullNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -216,7 +216,7 @@ class ConvexHullNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &ConvexHullNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -224,7 +224,7 @@ class ConvexHullNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -258,7 +258,7 @@ namespace convex_hull class ConvexHullNodelet : public opencv_apps::ConvexHullNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet convex_hull/convex_hull is deprecated, " "and renamed to opencv_apps/convex_hull."); diff --git a/src/nodelet/corner_harris_nodelet.cpp b/src/nodelet/corner_harris_nodelet.cpp index 5b9bbde7..a3e89d3b 100644 --- a/src/nodelet/corner_harris_nodelet.cpp +++ b/src/nodelet/corner_harris_nodelet.cpp @@ -182,7 +182,7 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet } } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -191,7 +191,7 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &CornerHarrisNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -199,7 +199,7 @@ class CornerHarrisNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -232,7 +232,7 @@ namespace corner_harris class CornerHarrisNodelet : public opencv_apps::CornerHarrisNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet corner_harris/corner_harris is deprecated, " "and renamed to opencv_apps/corner_harris."); diff --git a/src/nodelet/discrete_fourier_transform_nodelet.cpp b/src/nodelet/discrete_fourier_transform_nodelet.cpp index da46d04e..3305e208 100644 --- a/src/nodelet/discrete_fourier_transform_nodelet.cpp +++ b/src/nodelet/discrete_fourier_transform_nodelet.cpp @@ -83,7 +83,7 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet doWork(msg, msg->header.frame_id); } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -93,7 +93,7 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &DiscreteFourierTransformNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -185,7 +185,7 @@ class DiscreteFourierTransformNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -216,7 +216,7 @@ namespace discrete_fourier_transform class DiscreteFourierTransformNodelet : public opencv_apps::DiscreteFourierTransformNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet discrete_fourier_transform/discrete_fourier_transform is deprecated, " "and renamed to opencv_apps/discrete_fourier_transform."); diff --git a/src/nodelet/edge_detection_nodelet.cpp b/src/nodelet/edge_detection_nodelet.cpp index 82d5bf30..fc3ed1f2 100644 --- a/src/nodelet/edge_detection_nodelet.cpp +++ b/src/nodelet/edge_detection_nodelet.cpp @@ -273,7 +273,7 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -282,7 +282,7 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &EdgeDetectionNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -290,7 +290,7 @@ class EdgeDetectionNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -327,7 +327,7 @@ namespace edge_detection class EdgeDetectionNodelet : public opencv_apps::EdgeDetectionNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet edge_detection/edge_detection is deprecated, " "and renamed to opencv_apps/edge_detection."); diff --git a/src/nodelet/face_detection_nodelet.cpp b/src/nodelet/face_detection_nodelet.cpp index d3c5376c..371f6a8e 100644 --- a/src/nodelet/face_detection_nodelet.cpp +++ b/src/nodelet/face_detection_nodelet.cpp @@ -212,7 +212,7 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -221,7 +221,7 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &FaceDetectionNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -229,7 +229,7 @@ class FaceDetectionNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -276,7 +276,7 @@ namespace face_detection class FaceDetectionNodelet : public opencv_apps::FaceDetectionNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet face_detection/face_detection is deprecated, " "and renamed to opencv_apps/face_detection."); diff --git a/src/nodelet/face_recognition_nodelet.cpp b/src/nodelet/face_recognition_nodelet.cpp index 244dacd3..4088e78b 100644 --- a/src/nodelet/face_recognition_nodelet.cpp +++ b/src/nodelet/face_recognition_nodelet.cpp @@ -617,7 +617,7 @@ class FaceRecognitionNodelet : public opencv_apps::Nodelet config_ = config; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("subscribe"); img_sub_.subscribe(*it_, "image", 1); @@ -636,7 +636,7 @@ class FaceRecognitionNodelet : public opencv_apps::Nodelet } } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("unsubscribe"); img_sub_.unsubscribe(); @@ -644,7 +644,7 @@ class FaceRecognitionNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); @@ -676,7 +676,7 @@ namespace face_recognition class FaceRecognitionNodelet : public opencv_apps::FaceRecognitionNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet face_recognition/face_recognition is deprecated, " "and renamed to opencv_apps/face_recognition."); diff --git a/src/nodelet/fback_flow_nodelet.cpp b/src/nodelet/fback_flow_nodelet.cpp index 724e9868..5761ad3e 100644 --- a/src/nodelet/fback_flow_nodelet.cpp +++ b/src/nodelet/fback_flow_nodelet.cpp @@ -191,7 +191,7 @@ class FBackFlowNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -200,7 +200,7 @@ class FBackFlowNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &FBackFlowNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -208,7 +208,7 @@ class FBackFlowNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -242,7 +242,7 @@ namespace fback_flow class FBackFlowNodelet : public opencv_apps::FBackFlowNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet fback_flow/fback_flow is deprecated, " "and renamed to opencv_apps/fback_flow."); diff --git a/src/nodelet/find_contours_nodelet.cpp b/src/nodelet/find_contours_nodelet.cpp index 0e05b1bc..506320bd 100644 --- a/src/nodelet/find_contours_nodelet.cpp +++ b/src/nodelet/find_contours_nodelet.cpp @@ -202,7 +202,7 @@ class FindContoursNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -211,7 +211,7 @@ class FindContoursNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &FindContoursNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -219,7 +219,7 @@ class FindContoursNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -254,7 +254,7 @@ namespace find_contours class FindContoursNodelet : public opencv_apps::FindContoursNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet find_contours/find_contours is deprecated, " "and renamed to opencv_apps/find_contours."); diff --git a/src/nodelet/general_contours_nodelet.cpp b/src/nodelet/general_contours_nodelet.cpp index dc886720..b0b7803f 100644 --- a/src/nodelet/general_contours_nodelet.cpp +++ b/src/nodelet/general_contours_nodelet.cpp @@ -236,7 +236,7 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -245,7 +245,7 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &GeneralContoursNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -253,7 +253,7 @@ class GeneralContoursNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -289,7 +289,7 @@ namespace general_contours class GeneralContoursNodelet : public opencv_apps::GeneralContoursNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet general_contours/general_contours is deprecated, " "and renamed to opencv_apps/general_contours."); diff --git a/src/nodelet/goodfeature_track_nodelet.cpp b/src/nodelet/goodfeature_track_nodelet.cpp index 5c63924f..0fae7836 100644 --- a/src/nodelet/goodfeature_track_nodelet.cpp +++ b/src/nodelet/goodfeature_track_nodelet.cpp @@ -202,7 +202,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -211,7 +211,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &GoodfeatureTrackNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -219,7 +219,7 @@ class GoodfeatureTrackNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -254,7 +254,7 @@ namespace goodfeature_track class GoodfeatureTrackNodelet : public opencv_apps::GoodfeatureTrackNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet goodfeature_track/goodfeature_track is deprecated, " "and renamed to opencv_apps/goodfeature_track."); diff --git a/src/nodelet/hough_circles_nodelet.cpp b/src/nodelet/hough_circles_nodelet.cpp index 681c191a..f8346454 100644 --- a/src/nodelet/hough_circles_nodelet.cpp +++ b/src/nodelet/hough_circles_nodelet.cpp @@ -314,7 +314,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -323,7 +323,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &HoughCirclesNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -331,7 +331,7 @@ class HoughCirclesNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -378,7 +378,7 @@ namespace hough_circles class HoughCirclesNodelet : public opencv_apps::HoughCirclesNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet hough_circles/hough_circles is deprecated, " "and renamed to opencv_apps/hough_circles."); diff --git a/src/nodelet/hough_lines_nodelet.cpp b/src/nodelet/hough_lines_nodelet.cpp index 98fec48a..6e200014 100644 --- a/src/nodelet/hough_lines_nodelet.cpp +++ b/src/nodelet/hough_lines_nodelet.cpp @@ -267,7 +267,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -276,7 +276,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &HoughLinesNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -284,7 +284,7 @@ class HoughLinesNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -321,7 +321,7 @@ namespace hough_lines class HoughLinesNodelet : public opencv_apps::HoughLinesNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet hough_lines/hough_lines is deprecated, " "and renamed to opencv_apps/hough_lines."); diff --git a/src/nodelet/lk_flow_nodelet.cpp b/src/nodelet/lk_flow_nodelet.cpp index 0d17bb8c..867b804f 100644 --- a/src/nodelet/lk_flow_nodelet.cpp +++ b/src/nodelet/lk_flow_nodelet.cpp @@ -294,7 +294,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet return true; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -303,7 +303,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &LKFlowNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -311,7 +311,7 @@ class LKFlowNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -359,7 +359,7 @@ namespace lk_flow class LKFlowNodelet : public opencv_apps::LKFlowNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet lk_flow/lk_flow is deprecated, " "and renamed to opencv_apps/lk_flow."); diff --git a/src/nodelet/people_detect_nodelet.cpp b/src/nodelet/people_detect_nodelet.cpp index 45171780..ce94e2d3 100644 --- a/src/nodelet/people_detect_nodelet.cpp +++ b/src/nodelet/people_detect_nodelet.cpp @@ -192,7 +192,7 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -201,7 +201,7 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &PeopleDetectNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -209,7 +209,7 @@ class PeopleDetectNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -245,7 +245,7 @@ namespace people_detect class PeopleDetectNodelet : public opencv_apps::PeopleDetectNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet people_detect/people_detect is deprecated, " "and renamed to opencv_apps/people_detect."); diff --git a/src/nodelet/phase_corr_nodelet.cpp b/src/nodelet/phase_corr_nodelet.cpp index f6e17c39..df2f2c1b 100644 --- a/src/nodelet/phase_corr_nodelet.cpp +++ b/src/nodelet/phase_corr_nodelet.cpp @@ -184,7 +184,7 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -193,7 +193,7 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &PhaseCorrNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -201,7 +201,7 @@ class PhaseCorrNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -235,7 +235,7 @@ namespace phase_corr class PhaseCorrNodelet : public opencv_apps::PhaseCorrNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet phase_corr/phase_corr is deprecated, " "and renamed to opencv_apps/phase_corr."); diff --git a/src/nodelet/pyramids_nodelet.cpp b/src/nodelet/pyramids_nodelet.cpp index 8eeba0d4..469f48bf 100644 --- a/src/nodelet/pyramids_nodelet.cpp +++ b/src/nodelet/pyramids_nodelet.cpp @@ -148,7 +148,7 @@ class PyramidsNodelet : public opencv_apps::Nodelet } } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -157,7 +157,7 @@ class PyramidsNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &PyramidsNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -165,7 +165,7 @@ class PyramidsNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -197,7 +197,7 @@ namespace pyramids class PyramidsNodelet : public opencv_apps::PyramidsNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet pyramids/pyramids is deprecated, " "and renamed to opencv_apps/pyramids."); diff --git a/src/nodelet/segment_objects_nodelet.cpp b/src/nodelet/segment_objects_nodelet.cpp index 58e89d78..a6b276af 100644 --- a/src/nodelet/segment_objects_nodelet.cpp +++ b/src/nodelet/segment_objects_nodelet.cpp @@ -232,7 +232,7 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet return true; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -241,7 +241,7 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &SegmentObjectsNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -249,7 +249,7 @@ class SegmentObjectsNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -292,7 +292,7 @@ namespace segment_objects class SegmentObjectsNodelet : public opencv_apps::SegmentObjectsNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet segment_objects/segment_objects is deprecated, " "and renamed to opencv_apps/segment_objects."); diff --git a/src/nodelet/simple_compressed_example_nodelet.cpp b/src/nodelet/simple_compressed_example_nodelet.cpp index 610af320..c99605b5 100644 --- a/src/nodelet/simple_compressed_example_nodelet.cpp +++ b/src/nodelet/simple_compressed_example_nodelet.cpp @@ -226,7 +226,7 @@ note : hydro 1.10.18, indigo : 1.11.13 ... class SimpleCompressedExampleNodelet : public nodelet::Nodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { simple_compressed_example::ImageConverter ic; ros::spin(); @@ -240,7 +240,7 @@ namespace simple_compressed_example class SimpleCompressedExampleNodelet : public opencv_apps::SimpleCompressedExampleNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet simple_compressed_example/simple_compressed_example is deprecated, " "and renamed to opencv_apps/simple_compressed_example."); diff --git a/src/nodelet/simple_example_nodelet.cpp b/src/nodelet/simple_example_nodelet.cpp index c34a2c22..70b0ce85 100644 --- a/src/nodelet/simple_example_nodelet.cpp +++ b/src/nodelet/simple_example_nodelet.cpp @@ -120,7 +120,7 @@ class ImageConverter class SimpleExampleNodelet : public nodelet::Nodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { simple_example::ImageConverter ic; ros::spin(); @@ -134,7 +134,7 @@ namespace simple_example class SimpleExampleNodelet : public opencv_apps::SimpleExampleNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet simple_example/simple_example is deprecated, " "and renamed to opencv_apps/simple_example."); diff --git a/src/nodelet/simple_flow_nodelet.cpp b/src/nodelet/simple_flow_nodelet.cpp index a8b27743..c9629fe6 100644 --- a/src/nodelet/simple_flow_nodelet.cpp +++ b/src/nodelet/simple_flow_nodelet.cpp @@ -224,7 +224,7 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -233,7 +233,7 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &SimpleFlowNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -241,7 +241,7 @@ class SimpleFlowNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -276,7 +276,7 @@ namespace simple_flow class SimpleFlowNodelet : public opencv_apps::SimpleFlowNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet simple_flow/simple_flow is deprecated, " "and renamed to opencv_apps/simple_flow."); diff --git a/src/nodelet/smoothing_nodelet.cpp b/src/nodelet/smoothing_nodelet.cpp index c1d582a9..6d6bff8e 100644 --- a/src/nodelet/smoothing_nodelet.cpp +++ b/src/nodelet/smoothing_nodelet.cpp @@ -189,7 +189,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet prev_stamp_ = msg->header.stamp; } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -198,7 +198,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &SmoothingNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -206,7 +206,7 @@ class SmoothingNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -240,7 +240,7 @@ namespace smoothing class SmoothingNodelet : public opencv_apps::SmoothingNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet smoothing/smoothing is deprecated, " "and renamed to opencv_apps/smoothing."); diff --git a/src/nodelet/threshold_nodelet.cpp b/src/nodelet/threshold_nodelet.cpp index 35e59985..79fabf74 100644 --- a/src/nodelet/threshold_nodelet.cpp +++ b/src/nodelet/threshold_nodelet.cpp @@ -90,7 +90,7 @@ class ThresholdNodelet : public opencv_apps::Nodelet doWork(msg, msg->header.frame_id); } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -99,7 +99,7 @@ class ThresholdNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &ThresholdNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -147,7 +147,7 @@ class ThresholdNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -177,7 +177,7 @@ namespace threshold class ThresholdNodelet : public opencv_apps::ThresholdNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet threshold/threshold is deprecated, " "and renamed to opencv_apps/threshold."); diff --git a/src/nodelet/watershed_segmentation_nodelet.cpp b/src/nodelet/watershed_segmentation_nodelet.cpp index 97fada5f..c4465bf5 100644 --- a/src/nodelet/watershed_segmentation_nodelet.cpp +++ b/src/nodelet/watershed_segmentation_nodelet.cpp @@ -293,7 +293,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet } } - void subscribe() override + void subscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Subscribing to image topic."); if (config_.use_camera_info) @@ -302,7 +302,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet img_sub_ = it_->subscribe("image", queue_size_, &WatershedSegmentationNodelet::imageCallback, this); } - void unsubscribe() override + void unsubscribe() // NOLINT(modernize-use-override) { NODELET_DEBUG("Unsubscribing from image topic."); img_sub_.shutdown(); @@ -310,7 +310,7 @@ class WatershedSegmentationNodelet : public opencv_apps::Nodelet } public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { Nodelet::onInit(); it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); @@ -361,7 +361,7 @@ namespace watershed_segmentation class WatershedSegmentationNodelet : public opencv_apps::WatershedSegmentationNodelet { public: - void onInit() override + virtual void onInit() // NOLINT(modernize-use-override) { ROS_WARN("DeprecationWarning: Nodelet watershed_segmentation/watershed_segmentation is deprecated, " "and renamed to opencv_apps/watershed_segmentation.");